AD-A257  335 


NAVAL  POSTGRADUATE  SCHOOL 
Monterey ,  California 


DTIC 


EI.ECTE 
NOV  2  3  1992 


THESiS 


APPLICATION  OF  KALMAN  FILTER 
ON  MULTI SENSOR  FUSION  TRACKING 


by 


Brian  Everett  Terpening 
December  1992 


Thesis  Advisor: 


Hal  A.  Titus 


Approved  for  public  release;  distribution  is  unlimited 


UNCLASSIFIED 


ECURITV  CLASSiF'CATlON  Of  THIS  PAGE 


1a  REPORT  SECURITY  CLASSIFICATION 

UNCLASSIFIED 


2a  SECURITY  CLASSIFICATION  AUTHORITY 


2b  DECLASSIFICATION  /  DOWNGRADING  SCHEDULE 


a  PERFORMING  ORGANIZATION  REPORT  NUMBER(S) 


REPORT  DOCUMENTATION  PAGE 


lb  RESTRICTIVE  MARKINGS 


Form  Approved 
0MB  No  0704  0188 


3  DISTRIBUTION/AVAILABILITY  Of  REPORT 

Approved  for  public  release; 
distribution  is  unlimited 


S  MONITORING  ORGANIZATION  REPORT  NUMBER(S) 


6a  NAME  OF  PERFORMING  ORGANIZATION 

Naval  Postgraduate  School 


6c  ADDRESS  {City,  State,  and  ZIP  Code) 

Monterey,  CA  93943-5000 


6b  OFFICE  SYMBOL  7a  NAME  OF  MONITORING  ORGAMZATiON 
(If  applicable) 

EC  Naval  Postgraduate  School 


7b  ADDRESS  (C/ty,  State,  and  ZIP  Code) 

Monterey,  CA  93943-5000 


8a.  NAME  OF  FUNDING /SPONSORING 
ORGANIZATION 


8b  OFFICE  SYMBOL  I  9  PROCUREMENT  INSTRUMENT  IDENTIFICATION  NUMBER 
(If  applicable)  I 


10  SOURCE  OF  FUNDING  NUMBERS 


PROGRAM 
ELEMENT  NO 


PROJECT 

TASK 

NO 

NO 

8c.  ADDRESS  (C/fy.  State,  and  ZIP  Code) 


1 1  TITLE  (Include  Security  Classification) 

APPLICATION  OF  KALMAN  FILTER  ON  MULT I SENSOR  FUSION  TRACKING 


12.  PERSONAL  AUTHOR(S) 

TERPENING,  Brian  Everett 


13a  TYPE  OF  REPORT  1  3b  TIME  COVERED 

aster's  Thesis  


vVORK  UNIT 
ACCESSION  NO 


15  PAGE  COuN’ 

91 


17 

COSATI  CODES  1 

FIELD 

GROUP 

SUB-GROUP  1 

18  SUBJECT  TERMS  (Continue  on  reverse  if  necessary  and  identify  by  block  number) 

fusion,  Kalman  Filter  Multisensor  Fusion 
Tracking 


19  ABSTRACT  (Continue  on  reverse  if  necessary  and  identify  by  block  numberlj^hg  USB  of  Kalman  filtering  in  tracking 

targets  and  the  reconstruction  of  a  target's  track  are  addressed  in  two  separate  fusion 
schemes.  First,  the  Kalman  filter  is  used  to  provide  estimates  of  the  position  and  vel¬ 
ocity  of  a  target  based  upon  observations  of  the  target's  bearing.  Two  sensors,  a  radar 
in  receive  mode  and  an  infra-red  sensor,  take  bearings  to  the  target  at  different  scan 
rates.  This  information  is  then  fused  within  the  filter  to  obtain  the  target's  track. 
Secondly,  range,  bearing,  and  frequency  are  used  in  fusion.  Kalman  filtering,  Kalman 
smoothing,  and  maneuver  detection  are  all  used  in  the  reconstruction  of  a  target's 
track.  Improvements  are  Implemented  in  the  method  of  forcing  the  excitation  matrix  and 
the  results  documented. 


20  DISTRIBUTION/ AVAILABILITY  OF  ABSTRACT 
BD  unclassified/unlimited  □  SAME  AS  RPT 


22«  NAME  OF  RESPONSIBLE  INDIVIDUAL 

TITUS,  Hal  A. 


DO  Form  1473,  JUN  86 


21  ABSTRACT  SECURITY  CLASSIFICATION 

Q  DTic  USERS  UNCLASSIFIED  _ 


2?b  TELEPHONE  (Include  Area  Code) 

408-646-2560  _ 


PrewouJ  edffiOns  are  o6so/ef®  _ _ SECURITY  CLASSIFICATION  QP  This  PAGE 

S/N  0102-LF-014-6603  UNCLASSIFIED 


i 


Approved  for  public  release;  distribution  is  unlimited. 
Application  of  Kalman  Filter  on  Multisensor  Fusion  Tracking 

by 

Brian  E.  Terpening 
Lieutenant,  United  States  Navy 
BA.,  Louisiana  Tech  University 

Submitted  in  partial  ftilfillment 
of  the  requirements  for  the  degree  of 

MASTER  OF  SCIENCE  IN  ELECTRICAL  ENGINEERING 

from  the 


Author: 


Approved  by; 


NAVAL  POSTGRADUATE  SCHOOL 
December  1992 


/7r7.a/ryi^ 


Michael  A.  MorgjQlt  Chairman 
Department  of  Electrical  and  Computer  Engineering 


4 


U 


ABSTRACT 


The  use  of  Kalman  filtering  in  tracking  targets  and  the  reconstruction  of  a  target's  track  are 
addressed  in  two  separate  fusion  schemes.  First,  the  Kalman  filter  is  used  to  provide  estimates  of  the 
position  and  velocity  of  a  target  based  upon  observations  of  the  target’s  bearing.  Two  sensors,  a  radar 
in  receive  mode  and  an  infra-red  sensor,  take  bearings  to  the  target  at  different  scan  rates.  This 
information  is  then  fused  within  the  filter  to  obtain  the  target's  track.  Secondly,  range,  bearing,  and 
frequency  are  used  in  fusion.  Kalman  filtering,  Kalman  smoothing,  and  maneuver  detection  arc  all  used 
in  the  reconstruction  of  a  target’s  track.  Improvements  are  implemented  in  the  method  of  forcing  the 
excitation  matrix  and  the  results  documented. 
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I .  INTRODUCTION 


A.  TARGET  TRACKING 

The  detection, location,  and  tracking  of  targets  plays  a 
critical  role  in  many  aspects  of  military  operations.  The 
ability  to  accurately  track  and  predict  the  movement  of 
aircraft,  anti-ship  missiles,  and  torpedoes  in  military 
operations  can  not  be  overstated.  This  report  investigates 
the  use  of  multiple  sensors  and  Kalman  filtering  in  Fusion 
Tracking. 

The  Kalman  filter  has  been  in  use  for  over  three  decades 
in  aircraft  avionics,  radar,  sonar,  and  multiple  control 
problems.  This  investigation  covers  a  Kalman  filter  in 
tracking  a  target  by  bearing  observations  and  an  additional 
problem  using  bearing,  range,  and  frequency  observations. 
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II.  THE  KALMAN  FILTER 


The  Kalman  filter  estimates  the  state  of  a  linear  and 
time- invariant  (LTI)  system  given  a  set  of  known  inputs  to  the 
system  and  a  set  of  measurements. 

A.  DEFINITION  OF  TERMS 

The  terms  used  in  this  chapter  and  Appendix  B,  are  defined 
in  Table  (2.1).  Kalman  filter  system  dynamics  utilize  state 
space  representation,  therefore  matrix  dimensions  are  given. 
TABLE  2.1:  Definition  of  Terms 


Error  covariance  matrix: 
Estimate  error : 

Expected  value  of  error: 
Identity  matrix: 

Kalman  gain  matrix: 
Measurement  matrix: 
Observation: 

Observation  noise: 
Residual: 

State  estimate: 

State  excitation  noise: 
State  transition  matrix; 
System  state: 

Transpose: 


Dimension  Code  Symbol 


n 

X 

n 

P 

n 

X 

1 

n 

X 

1 

n 

X 

n 

eye(n) 

I 

G(k) 

-G, 

n 

X 

1 

H 

H 

theta (k) 

Z| 

1 

X 

1 

R 

V| 

1 

X 

1 

r. 

n 

X 

1 

XKK 

Q 

n 

X 

n 

Phi 

n 

X 

1 

X 

X' 

X 

Appendix  A  defines  the  terms  used  in  the  Kalman  filter 
designed  around  the  torpedo  tracking  problem.  This  program  is 
discussed  further  in  Chapter  IV. 
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B.  EQUATIONS  AND  INITIALIZATION 

The  background  given  here  coincides  with  that  of  [Ref  1] . 
The  equations  for  the  filter  used  in  this  program  are  listed 
below. 


(2.1) 

(2.2) 

(2.3) 

(2.4) 

(2.5) 

(2.6) 

Terms  with  a  single  time  sxibscript  (i.e.,  x,^)  refer  to  the 
value  of  the  term  at  that  time.  Those  terms  with  dual 
subscripts  (i.e.,  P(k*ivk))  refer  to  the  value  of  the  term  at  a 
future  time  given  the  present  states. 

The  Kalman  filter  requires  initialization  and  the  proper 
choice  of  initialization  is  important.  The  value  of  is 

initially  set  to  zero  and  the  following  steps  are  then 
evaluated  in  the  recursion  algorithm. 

•  Use  Equation  2.1  to  calculate  the  Kalman  gain, 

•  Use  Equation  2.2  and  2.3  to  evaluate  the  values  of  the 
covariance  of  estimate  error  matrix  for  the  next 
iteration. 

•  Use  Equation  2.4  to  find  the  state  estimates. 

•  Use  Equation  2.5,  given  the  current  state  estimates  to 
project  ahead  for  use  in  the  next  iteration. 
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In  the  program,  the  bearing  theta  is  simulated  and  sampled  by 
the  filter  as  if  they  were  taken  from  a  sensor. 
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III.  THE  MODEL 


In  this  chapter,  we  introduce  the  mathematical  and 
physical  model  for  our  first  tracking  problem.  We  start  by 
presenting  the  physical  relationship  between  the  target  and 
the  observer.  After  the  track  has  been  generated,  we  use  the 
Kalman  filter  to  verify  that  the  mathematical  model  is 
tracking  the  physical  model. 


A.  PHYSICAL  SYSTEMS 


The  first  system  used  in  this  investigation  consists  of  a 
single  observer  and  a  single  target.  For  the  generation  of 
the  target's  track,  a  two-dimensional  Cartesian  coordinate 
system  with  the  observer  at  the  origin  is  used.  When 
referring  to  Figure  3.1,  this  x-y  grid  can  be  thought  of  as 
looking  down  on  a  north-south/east-west  grid.  The  target  is 
free  to  move,  unrestricted,  throughout  the  coordinate  space 
while  the  observer  is  stationary  and  remains  at  the  origin. 

The  information  received  by  the  observer  consists  of 
bearing  to  the  target.  Therefore,  the  state  space  estimate 
appears  as: 


e 

6 

eJ 


(3.1) 


5 


i 

Y 

vt 

c 

e 

/  Target  vx 

y  Observer 

X 

Figure  3.1;  First  Physical  System 


These  bearings  are  to  be  consistent  with  measurements  supplied 
by  a  phased  array  radar,  in  receive  only  mode,  and  most  types 
of  infra-red  scanners  used  today.  The  measurements  are  taken 
from  the  radar  100  times  per  second,  while  the  infra-red  is 
sampled  every  second. 

B.  MODEL  TEST 

It  is  critical  that  the  recursive  program  receive  its 
initial  states  and  observations  in  the  proper  order. 
Therefore,  we  will  examine  the  Kalman  gains  for  a 
verification.  The  MATLAB  programs  MISSILEl.M  and  MISSII1E2.M 
used  are  found  in  Appendix  B.  For  the  purpose  of 
verification,  we  will  assume  no  noise  with  the  covariance  of 
measurement  noise  and  covariance  of  excitation  equal  to  zero. 
If  the  filter  is  working  properly,  the  Kalman  gains  for  the 
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first  sample  should  have  the  position  vector  G(l,l)  equal  to 
one.  The  velocity  and  acceleration  vectors,  G(2,l)  and  G(3,l) 
respectively,  should  equal  zero  (see  Figures  3.9  through 
3.11).  At  the  second  sample,  G(l,2)  is  given  by  ^(2)  -  z(2)  , 
G(2,2)  is  given  by  (l/T)  [z(2)-z(l)]  ,  and  G(3,n)  should  remain 
zero.  For  the  third  sample,  G(3,3)  should  take  on  a  value 
given  by 

(l/r2){[z(3)-z(2)]-[z(2)-z(l)]}.  (3.2) 

For  this  simulation  the  target  has  v^  =  1  km/sec  as  shown 
in  Figure  3.3.  Figures  3 . 4  and  3 . 5  show  the  distance  traveled 
and  change  in  bearing  over  one  second.  The  figure  showing  the 
change  in  bearing  might  be  deceptive,  as  it  appears  to  be  a 
straight  line.  In  fact  it  is  a  curved  line  that  reaches  its 
maximum  rate  of  change  when  the  observer  is  perpendicular  to 
the  target’s  path.  Figures  3.6  through  3.8  show  the  state 
estimates  of  the  Kalman  filter,  that  gives  us  a  comparison  to 
the  actual  change  in  bearing. 

Figures  3.9  through  3.11  are  the  Kalman  gains.  However, 
the  covariance  of  measurement  noise  is  0.001.  This  avoids  a 
divide  by  zero  in  the  Kalman  gain  equations.  This  will  have 
a  minimal  effect  on  the  results. 
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Figure  3.5:  State  estimate  of  bearing 
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Figure  3.6:  State  estimate  of  change  in  rate 
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Figure  3.7:  State  estimate  of  angular  acceleration 
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Figure  3.8;  Kalman  filter  gain  vector  G(l,k} 
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Figure  3.9:  Kalman  filter  gain  vector  G(2,k} 
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IV.  TBE  KALMAM  FILTER  WITH  MAMEDVER  DETECTION 


A.  PROGRAMS 

This  chapter  is  devoted  to  an  explanation  of  the 
components  and  logic  used  for  the  programs  in  Appendix  C. 
TORPMAN.FOR  uses  the  combined  work  from  [Ref  l]  and  Raymond  M. 
Alfaro.  In  order  to  run  the  program,  follow  this  procedure. 

•  Run  TORPSCUR.M  for  the  s-curve  or  TRACKS IM.M  for  the 
radial  track  as  in  [Ref  1] . 

•  GENFILES.M  converts  the  track  to  a  data  format. 

•  POSCONV.FOR  converts  the  MATLAB  (ASCII)  to  FORTRAN 
(keyport)  input. 

•  Run  OBSDATA.FOR  to  covert  position,  velocity, 
acceleration,  time,  and  roll  data  files  to  a  single 
observation  file  OBSERVAT.DAT  . 

•  TORPMAN.FOR  is  the  Kalman  filter  and  may  be  analyzed  by 
using  ANALYZE. M  . 

B.  STATE  SPACE  AMD  OBSERVATION  MATRICES 

As  stated  previously,  this  problem  utilizes  the  cartesian 
coordinate  system.  The  state  space  is  made  up  of  position, 
velocity,  and  acceleration  in  the  x,  y,  and  z  vector  space, 
where  the  dot  denotes  a  time  derivative. 

X  ^  [X  k  k  y  y  ^  z  i  2]^  (4.1) 
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1  t 


0  0 


0  0  0 


tf 

2 

0  1  too 

0  0  10  0 


0  0 


♦  = 


0 

0 


0 

0 


0  1  t 

0  0  1 
0  0  0 


0  0  0 
0  0  0 


too 
10  0 


0 

0 

0 

0 

0 

0 


0  0  0 

0  0  0 
0  0  0 


0  0  0 

0  0  0 
0  0  0 


0  1  t 
0  0  1 


(4.2) 


The  observation  measurements  for  our  system  consists  of 
position  and  velocity  measurements  with  or  without  noise 
added.  Observation  measurements  are  defined  in  the  following 
manner; 


X 

X 


We  are  assuming  the  acceleration  information  is  not  available. 
However,  if  it  were  possible  to  get  the  observation,  the 
filter  would  take  advantage  of  the  opportunity  to  improve  its 
estimate.  We  will  also  assume  that  some  observation  may  have 
missing  position  and  velocity  data.  This  will  be  discussed  in 
depth  later  in  the  chapter. 


12 


1 .  Physical  Model 

For  the  system  as  shown  in  Figure  4.1,  the  Cartesian 
coordinate  system  is  used.  This  implies  an  array  of  sensor 
generate  the  range  measurements.  Again  the  target  is  free  to 
move  throughout  the  coordinate  space.  However,  the  graphic 
information  displays  only  X-Y  coordinates.  Frequency 
information  is  also  utilized  in  the  velocity  estimate  by 
estimating  the  turn  counts. 


C.  WEIGHTED  COVARIANCE  MATRIX 

The  covariance  of  excitation  error  matrix,  Q(k),  is 
considerably  more  complex.  The  matrix  can  be  weighted  or 
manipulated  when  a  maneuver  is  detected.  In  this  new 
arrangement,  the  x,  y,  and  z  coordinates  are  adjusted 
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independently  of  the  others.  This  matrix  was  derived  in  [Ref 
2,  pp.  36-42]  and  is  given  below. 


The  expression  for  distance  is 


where 

t  =  sample  time  interval, 
w  =  weighting  factor,  and 

s  =  distance  travelled  in  one  sample  interval. 

A  good  example  of  finding  w  is  given  in  [Ref  1,  pp.  22].  One 
should  be  reminded,  when  solving  for  w,  it  is  the  worst  case 
and  a  smaller  weight  is  more  robust. 
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1.  Error  Ellipsoids 


Error  ellipsoids  are  useful  in  visualizing  the  margins 
of  error.  The  state  value  should  lie  vithin  a  certain  region 
surrounding  the  estimate.  This  uncertainty  is  expressed  in 
the  covariance  of  error  matrix  P|jy,^. 

The  position  components  submatrix  of  the  error  covariance 


matrix  is  defined  as 


(p  p  \ 

•^11  -^14 

-f 

^■^41  •^44; 

\ 

_  f  var  X  cov{x,y)\ 
\cov(x,y)  var  y 


f  2 

2  \ 

\  - 

Ox 

°xy 

_2 

f 

Oxy 

°y) 

(4.6) 


The  diagonal  terms  P,,  and  P^^  of  the  covariance  matrix 
represents  the  variances  of  the  estimate  in  the  x  and  y 
positions  respectfully.  If  P,^^^  is  a  nonnegative  definite 
matrix  and  the  random  Gaussian  noise  processes  w(k)  and  v(k) 
are  independent,  a  surface  of  constant  probability  density  can 
be  produced.  To  elaborate  on  what  is  desired,  Figure  4.2  is 
provided. 
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Figure  4.2:  Dimensions  for  the  error  ellipse 


a.  Matrix  Method 

From  the  density  function 

a-x^Px 
c?  i 

we  specify  the  level  surface  about  the  origin  by 

*  1  . 

From  numerical  analyses  [Ref  3,  pp.  164],  there  is 
upper  triangular  matrix  R  such  that 


(4.7) 


(4.8) 
a  unique 


P  =  R'^R  (4.9) 

Moreover,  this  matrix  can  be  stably  computed  without  pivoting. 

This  result,  however,  is  equivalent  to  stating  that  there  is 

a  corresponding  unique  triangular  matrix  L(=R^)  such  that 

P  =  LL^  (4.10) 

In  terms  of  the  matrix,  our  basic  problem  4.8  becomes 


x’'(LL*’)-^x  =  1 


(4.11) 


However,  elementary  matrix  properties  imply  that 
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(4.12) 


=  (L-^X)^  (L-^X)  =  [L-^xil 
where  |  I2  represents  the  usual  Euclidean  norm.  But,  this  last 


equality  implies  that 

x^p-^x  =1  «  =  1 

Therefore,  the  basic  problem  4. 10  is  equivalent  to 


[L^x[,  =  1  . 

This  equation  is  solvable  if  and  only  if, 

L~^x  =  y  for  some  vector  y  such  that  \y\2  =  1 

But, 


(4.13) 


(4.14) 


(4.15) 


L‘^x  =  y  **  X  =  Ly  .  (4.16) 

Therefore,  we  conclude  that  the  desired  error  ellipsoid  is 
specified  by 

{  xlx^^Ly  ,  lyllj*!  )  (4.17) 

b.  Reaction  to  a  maneuver 

The  effects  of  maneuver  detection  are  seen  in 
Figure  4.3.  As  the  filter  detects  an  acceleration  in  one 
direction,  the  uncertainty  of  the  position  increases  in  that 
dimension.  The  ellipsoids  can  be  used  to  verify  the  filters 
ability  to  predict  the  movements  of  the  target.  The  program 
ELLIPSE. M  that  generates  the  ellipsoids  can  be  found  in 
Appendix  B. 

c.  Initialization  check 

The  program  S_ELLIPSE.M  contained  in  Appendix  C 
give  us  the  same  visual  verification  of  the  filter  using  the 
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Figure  4.3:  The  effect  of  maneuver  detection  on  the 
covariance  of  error 


same  idea.  By  using  the  error  covariance  matrix  P  we  can 
obtain  a  visual  insight  into  the  performance  of  the  filter. 
Figure  4.4  shows  the  exponential  decay  of  the  probability  of 
error. 

D.  THE  OBSERVATION  MATRIX  -  H 

The  logic  for  the  observation  matrix  starts  with  the 
manipulation  of  data  in  OBSDATA.FOR,  in  Appendix  C.  After 
receiving  an  observation,  the  data  is  manipulated  to  obtain  a 
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Trajectory  —  S  Curve 


X  -  Position  (ft) 

Figure  4.4:  The  correct  response  for  the  covariance  of 
error  ellipsoids 


state  space  vector  with  position,  velocity,  and  acceleration. 

Based  on  which  state  variables  are  actually  present  in  the 

observation,  a  unique  binary  signal  is  generated.  Take  the 

coefficient  of  the  power 

..j  .  1  if  is  present  in  the  observation 

^  ~  '  0  if  x^  is  not  present 

The  number  is  stored  with  the  position  vector  (x)  as  low  bit. 
For  example: 
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The  binary  nvunber  implies  that  x,  ^  and  y  make  up  the 
observation.  This  allows  subsequent  decoding  of  which 
components  are  present  by  a  combination  of  divide  by  2  and 
modulo  opus. 

The  matrix  H,  can  be  produced  for  any  combination  of 
measurements.  Given  the  same  statistical  characteristics  as 
the  actual  data,  it  is  possible  to  have  missing  data  and 
multiple  returns  from  adjacent  arrays.  This  filter  has  the 
ability  to  continue  when  pieces  of  data  are  not  received,  or 
when  the  two  sensors  are  operating  at  slightly  different 
frequencies.  The  scheme  used  here  helps  in  this  regard  in 
that  it  is  more  robust. 
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V.  RE80LT8 


A.  BA8ELI11B  INFORMATIOM 

The  s-curve  data  is  run  through  the  filter  from  [Ref  1, 
pp.  61] .  This  information  will  now  be  used  in  determining 
improvements  in  performance.  The  knowledge  that  the  filter 
performed  best  with  the  excitation  matrix  weighted  to  1500, 
is  utilized  in  the  production  of  Figure  5.1.  The 


Figure  5.1:  X  and  Y  smoothed  estimate  error  with  old 
maneuver  detection  scheme 


performance  of  the  filter  can  be  improved  in  this  noiseless 
environment  by  increasing  the  figure  for  the  weighing  from 
1500.  However,  as  shown  in  [Ref  l] ,  the  filter's  performance 
in  a  noisy  environment  would  be  degraded. 


2 


For  a  better  idea  of  what  is  causing  the  errors.  Figure 
5.2  shows  when  the  target  is  maneuvering.  Looking  at  where 
the  maneuvers  occur,  coincides  with  the  disturbance  in 
tracking. 


B.  MODIFICATIONS 

The  filter's  maneuver  detection  scheme  and  observation 
measurement  matrix,  (H)  ,  are  now  as  discussed  in  the  previous 
chapter.  This  filter  is  modified  in  one  other  respect.  This 
filter  manipulates  the  excitation  matrix  when  a  maneuver  is 
detected,  not  after  the  maneuver  is  detected.  The  purpose  of 
estimating  the  acceleration  is  to  be  able  to  sense  the 
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maneuver  simultaneously  with  the  onset  of  the  maneuver.  "None 
of  the  time  lag  associated  with  the  residual  method  is 
experienced."  [Ref  1]  The  results  are  readily  seen  in  Figure 
5.3.  Again,  this  is  with  no  noise  and  the  Q  matrix  is 
weighted  at  unity.  A  view  of  the  targets  path  is  provided  in 
Figure  5.4. 


Figure  5.3:  X  and  Y  error  for  the  s-curve  using  the  new 
filter;  weight  is  set  at  unity 


Note  that  the  smoothing  in  backward  filter  causes  the 
larger  of  the  errors  in  a  noiseless  environment.  The  more  the 
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Figure  5.4:  The  targets  track  in  feet 

excitation  matrix  is  weighted  the  less  the  errors  become. 
But,  again,  this  does  not  hold  true  with  noise  in  the 
environment  and  the  trade  offs  need  to  be  evaluated. 

C.  EVALUATION  FOR  OPTIMAL  PERFORMANCE 

In  order  to  evaluate  the  differences  in  the  two  filters, 
we  must  first  establish  the  optimal  performance  for  this  new 
filter.  Multiple  tracks  are  created  to  establish  a  value  for 
the  weight  and  to  gather  enough  data  to  reduce  any  statistical 
errors . 

As  discussed  in  [Ref  1,  pp.  31]  Gaussian  noise  is  added  to 
the  track  with  a  distribution  variances  of  15  square  feet, 
0.01  square  yards/second,  and  0.03  square  feet/second  squared 
for  position,  velocity,  and  acceleration  respectively,  with 


500  1000  1500  2000  2500  3000 


X— exta 


24 


zero  mean.  A  figure  of  such  a  track  is  provided  in  Figure 


5.5. 


X  (-)  and  yrondont  ( — ) 


j _ _ 

Figure  5.5:  Target  track  with  noise  added 


Trial  and  error  is  used  to  find  the  optimal  performance  of 
the  filter.  The  numbers  seen  in  Table  5.1  are  the  mean  radial 
distance  errors  obtained  from  the  program  in  [Ref  1,  pp.  100]. 
Table  5.1  shows  a  few  of  the  multiple  iterations  of 
combinations  used  in  choosing  the  optimal  weights.  The 
maximiim  weight  (WTMAX)  and  minimum  weight  (VTTMIN)  are  chosen 
to  be  100  and  0.1  respectively.  This  choice  offers  a  half 
foot  reduction  in  mean  radial  distance  error  in  a  relatively 
quiet  environment,  while  giving  up  a  little  less  than  a  half 
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foot  in  this  noise  filled  environment.  Figure  5.6  shows  how 
the  filter  handles  a  noisy  track  for  one  run. 


TABLE  5.1:  Table  of  weights 


Maximum  Weight 

500 

100 

10 

1 

0.1 

0.01 

* 

100 

4.27 

4.03 

— 

— 

— 

— 

10 

3.18 

2.86 

2.3 

— 

— 

M 

1 

3 

2.67 

2.3 

2.23 

— 

— 

I 

N 

0.1 

2.96 

2.66 

2.28 

2.20 

2.22 

— 

.01 

2.98 

2.67 

2.28 

2.22 

2.31 

3.36 

Figure  5.6;  Error  for  noisy  track 


O.  COMPARISONS 

Both  filters,  using  all  of  their  best  features,  are  used 
on  a  track  with  approximately  20  feet  of  mean  radial  distance 
error.  Figure  5.7  provides  a  visual  comparison  as  to  the 
capability  of  each  filter  in  the  same  figure.  The  mean  radial 
distance  errors  in  the  upper  left  hand  corner  show  the 
difference  in  performances  of  the  old  and  new  filters. 
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Figure  5.7;  A  comparison  between  mean  radial  distance  of  the 
old  and  new  filter 

For  the  circular  maneuver  presented  in  [Ref  l,pp.  36],  we 
will  look  at  the  best  result  for  the  old  filter  with  optimal 
settings.  In  this  track  the  torpedo  makes  360  and  270  plus 
degree  turns.  The  old  mean  radial  distance  error  was  5.951  . 
In  Figure  5.8,  the  mean  radial  distance  errors  are  in  the 
upper  left  hand  corner.  The  old  track  is  the  "raw” 
observation  while  the  new  track  is  the  smoothed  filter 
estimate  of  the  true  track.  The  mean  radial  distance  error 
show  over  a  50%  improvement  with  an  error  of  2.165  . 
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Figure  5.8;  Filter  output  with  WTMIN=0.1  and  WTMAX=100.0 
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VI.  CONCLUSIONS  AND  RECOMMENDATIONS 


The  objective  was  to  investigate  uses  of  fusion  in 
tracking  problems.  In  the  process,  there  were  several  items 
that  bore  out  special  investigation. 

The  program  for  torpedo  track  reconstruction  now  works  in 
three  dimensions  and  its  overall  performance  improved.  One 
major  improvement  was  the  creation  of  the  OBSDATA.FOR  program 
and  its  subsequent  improvement  on  the  observation  or 
measurement  matrix.  This  revamping  of  the  program  and  its 
logic  has  improved  its  overall  characteristic  another  50  to 
80%. 

The  tracking  of  targets  by  electromagnetic  and  infra-red 
sensors  needs  to  be  pursued.  The  sensors  in  [Ref  4],  all  have 
the  capability  of  tracking  fast  moving  targets.  The 
preliminary  results  look  good  for  three  polar  coordinate 
filters,  all  working  in  parallel,  to  track  air  traffic 
passively.  If  there  were  one  filter  per  sensor,  and  one 
filter  to  fuse  and  triangulate  bearings  using  a  conversion 
vector  like  in  [Ref  4,  pp.4-4],  then  vessels  would  be  able  to 
track  traffic  passively  in  conditions  of  restricted 
electromagnetic  emissions. 
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APPENDIX  A:  DEFINITION  OF  TERMS  FOR  USE  IN  TORPMAN.FOR 


The  terms  and  characters  defined  here  are  for  easy  access 
to  those  trying  to  comprehend  the  program  in  Appendix  C.  The 
logic  for  the  program  is  discussed  in  more  depth  in  Chapter 
IV. 

TABLE  A.l:  Terms  used  in  TORPMAN.FOR 


$3DTEMP 

$M1TEMP 

ADD 

ACCTHRSH 

CHANGE 

H 

NDIMEN 


This  is  a  direct  access  file  for  storing 
values  of  the  error  covariance  matrix  (Pi^^i^)  . 
The  character  associated  with  this  file  is 
PKKS3D. 

This  is  a  direct  access  file  for  storing 
values  of  PKKMIS  or  the  smoothed  Pit^i^.i. 

A  subroutine  which  adds  two  input  matrices. 

The  acceleration  threshold  is  initialized  to 
5  feet/second.  This  is  for  use  in  the 
maneuver  detection  program,  MANDET,  and  may 
be  changed  within  the  SETTHRSH  subroutine. 

This  is  a  subroutine  which  allows  the  user  t 
change  the  weight  (WTMIN  or  WTMAX)  and  error 
covariance  matrix  at  time  k  minis  1  (PKKMl) . 
The  program  is  therefore  altered  in  behavior 
without  having  to  compile. 

The  measurement  matrix  is  discussed  in  more 
depth  in  chapter  V. 

The  number  of  dimensions  used  are  1,  2,  or  3 
D. 


NUMSTA  This  is  the  number  of  states  in  the  vector 

space . 

NSTMAT  This  is  used  in  FORTRAN  memory  allocation  fr 

one  time  step. 
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NOBVEC 

OBSERVAT.DAT 

PHIDEL 

PKK 

PKKMl 

PKKMIS 

PKKOUT.DAT 

Q 

R 

ROLTHRSH 

SCR 

TMPVl  &  2 

TEMPI,  2,  &  3 

WTMAX 

WTMIN 

XKK 


This  allocates  an  array  large  enough  for  the 
observed  data  set. 

The  observed  data  base. 

This  subroutine  builds  the  Phi  and  Del  ]iBb±e. 

The  error  covariance  matrix  (Pi^yi^)  . 

The  error  covariance  matrix  (Pk^k.i)  /  which  is 
at  time  minus  one. 

This  is  the  smoothed  PKKMl  from  the  backward 
Kalman  filter. 

An  output  file  is  created  containing  the 
information  for  error  ellipse  in  the  x  and  y 
plane. 

State  excitation  noise  matrix. 

Observation  noise  matrix. 

The  roll  threshold  is  initialized  to  5 
degrees/ second.  This  can  be  changed  with  te 
SETTHRSH  subroutine. 

An  integer,  when  read  as  a  binary  number, 
reveals  the  make  up  of  the  observation. 

A  temporary  storage  space  for  characters  as 
they  change  value  from  k/k-1  to  k/k  to  k+l/k 

*  k/k-1  . 

A  temporary  storage  space  for  characters  as 
they  change  value  from  k/k-1  to  k/k  to  k+l/k 

*  k/k-1  . 

This  is  the  maximum  weight  used  upon  the 
detection  of  a  maneuver.  The  value  is  preset 
to  20,000  but  can  be  altered  in  the  CHANGE 
subroutine . 

This  is  the  minimiun  weight  used  in  the 
program.  Which  insures  that  the  Kalman  gains 
do  not  approach  zero  if  the  target  is  not 
maneuvering . 

The  state  estimate 
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XKKMl 

XKKFWD.DAT 

XKKS 

XXSMOOTH.DAT 

ZZ 


The  state  estimate  which  is  at 

time  minis  one. 

The  state  estimates  of  the  Kalman  filter  are 
stored  in  this  data  file. 

Observations  are  stored  in  this  matrix, 
including  acceleration  obtained  from  AV.  It 
is  then  used  for  the  state  estimate  and 
smoothed  state  estimate  as  it  passes  through 
forward  and  backward  Kalman  filter. 

The  state  estimates  of  backward  filtering 
(XKKS)  are  stored  in  this  file. 

Stores  the  observations. 
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APPBMDZZ  B:  MATLAB  PROaRANS 


A.  ANALYZE. M 

%analyze.m 

%  This  M-file  compares  the  output  of  a  Kalman  Filter  Rim 

%  with  the  true  x,  y,  and  z  positions. 

clg 

!del  data\six.met 
!  cd  data 
load  tracknum.dat 
tracknum  =  fix(tracknum)  ; 
load  trueposn. 
load  xkkfwd.dat 
load  xksmooth.dat 
% 

errfwd=xlckf wd  ( : ,  2 : 4 )  -trueposn  ; 
errbwd=xksmooth ( : , 2 : 4 ) -trueposn  ; 

% 

eval(  [  '!  del  filtr' ,num2str (tracknum) .met'  ]) 
subplot (2 11)  ; 

plot (xkkfwd (: ,1) , errfwd ( : , 1) , ' — g' , . . 

xkkfwd(;,l),  errbwd( : , 1) , '-.w'  ) 
xlabel('Time  (sec) ') ;ylabel ( 'X-Error  Magnitude'  ); 
title (' Forward  ( — )  and  Backward  (-.)  X  Error  vs.  Time'); 

% 

plot(xkkfwd( : , 1) ,errfwd( : ,2) , ' — g' , . . 

xkkfwd( : , 1) ,errbwd( : ,2) , '-.w' ) 
xlabel ( 'Time  (sec) ' ) ;ylabel ( 'Y-Error  Magnitude'  ) ; 
title (' Forward  ( — )  and  Backward  (-.)  Y  Error  vs.  Time'); 
eval(  [  'meta  filtr ' ,num2str( tracknum)  ]) 
meta  six 
pause 
% 

clg 

subplot (211) 

plot (xkkfwd ( ; , 1) , errfwd ( : , 3 ) , ' — g ' , . . 

xkkfwd(: ,1) ,errbwd(: ,3) , '-.w') 
xlabel ( ' Time  ( sec) ' ) ;ylabel ( ' Z-Error  Magnitude '  ) ; 
title (' Forward  (— )  and  Backward  (-.)  Z  Error  vs.  Time'); 
%meta 
pause 
clg 
% 

eval ( ' !  cd  . . ' ) 
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B.  ELLIPSE. M 


This  program  will  plot  an  error  ellipsoid  once  ever  six 
observations . 

!del  pic2.met 

load  c : \matlab\wiseman\output2 . dat 
load  c : \matlab\wiseman\truepos2 . dat 
load  c:\matlab\wisemanN^lckout.dat 
axis ( ' square ' ) 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  plot  path  and  estimate  of  path  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
plot (output2 ( : , 1) , output2 ( : , 2) , , 

truepos2 {i ,2) , truepos2 ( : , 3 ) ) 
xlabelCX  -  Position  (ft)  ' )  ;ylabel  ( *  Y  -  Position  (ft)') 
title ( 'Trajectory  -  S  Curve') 

gtext(' _  True  track') 

gtext('+  +  +  Estimate') 
gtext('0  Error  ellipsoid') 
gtext('20  times  larger') 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  generate  a  uniformly  spaced  set  of  points  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
delt=“l: .05:1; 
delt**pi*delt; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  generate  a  (2xm)  set  of  vectors  on  the  unit  circle  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

y»[cos(delt) ;sin(delt) ] ; 

shift=diag(ones(41) ) ; 

shift^shift ' ; 

hold  on 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


%  The  function  chol,  to  compute  the  Choleski  % 
%  decomposition,  is  done  "backwards"  in  NATLAB.  It  % 
%  makes  an  upper  triangular  matrix  R,  not  L.  So  take  % 
%  the  transpose.  % 
%  An  array  of  x-vectors  (2xm)  has  been  created  that  % 
%  form  the  error  ellipsoid.  % 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
for  k«6:6:max(size(output2) ) , 

P*pkkout (k*2-l;k*2, 1:2) ; 

sschol (P) ' ; 

x*s*y; 

plot(x(l, : ) *20+output2 (k,l) *shift, 

x(2, ; ) *20+output2 (k, 2) *shift) 

end 

hold  off 
axls( 'normal ' ) 
meta  pic2 
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C.  GENFXLES.M 


%  THIS  PROGRAM  CONVERTS  SIMULATED  TORPEDO  TRACKS  GENERATED  BY 
%  THE  MATLAB  PROGRAM  TORPGEN.M  INTO  FILES  SUITABLE  FOR  INPUT 
%  TO  THE  FORTRAN  PREPROCESSOR  OBSDATA.FOR. 

%  AFTER  CONVERSION  INTO  STANDARD  INPUT  FORMAT  BY  OBSDATA.FOR, 
%  THE  DATA  WILL  BE  TESTED  AGAINST  THE  KAUIAN  FILTERING  PROGRAM 
%  TORPMAN.FOR,  WHICH  IS  DESIGNED  TO  RECONSTRUCT  TORPEDO  TRACKS 
%  GENERATED  AT  THE  UNDERSEA  RANGE  AT  KEYPORT,  WA. 

% 

% 

% 

%%%%%%%  Prompt  User  for  Desired  Type  and  Run  %%%%%%%%%%%%%% 
%%%%%%%  Number  for  Data  File  %%%%%%%%%%%%%% 

% 

!  els 

dispC  Select  Type  of  Simulated  Track  to  Process  •  ) 
disp('  ') 

disp('  1.  True  Track  (w/o  pseudo-random  noise  added)'  ) 

disp('  2.  Track  with  Pseudo-Random  Noise  Added'  ) 

disp('  (Any  other  value  will  abort  processing)') 
disp('  ') 

ntrktyp  =  input ('  Selection:  ')  ; 

if  ntrktyp  <=  0  ;  return  ;  end 
if  ntrktyp  >  2  ;  return  ;  end 
% 

dispC  '); 

trutrk  =  input  ([' Enter  the  Sequence  Number  of  the  Tirue  Track 

File:  '])  ; 

obsfil  =  [ 'true' ] ; 
obstrk  =  trutrk; 
if  ntrktyp  ==  2 
disp ( '  ' ) ; 

obsfil  =  ['tst']; 

obstrk  =  input ([' Enter  the  Sequence  Number  of  the  Simulated 

Track  File:  '])  ; 

end 


% 

%%%%%%%%%%  Input  the  Desired  Files  and  Produce 
%%%%%%%%%%  Standard  Output  Files  That  Will  Run 
%%%%%%%%%%  with  Program  POSCONV.FOR 

%  trueposf  =  [ 'truep' , int2str( trutrk)  ]  ; 

deltatf  *  [ 'delt' , int2str (trutrk)  ]  ; 
eval(  [  'load  data V r trueposf  ]) 
eval(  [  'load  data\' , deltatf  ]) 

% 

save  data\trueposn  x  /ascii 

save  data\deltsimu  deltat  /ascii 

% 

positionf  =  [  obsfil, 'p' , int2str (obstrk) 
velocityf  =  [  obsfil, 'v' ,int2str (obstrk) 


%%%%%%%%%% 

%%%%%%%%%% 

%%%%%%%%%% 


]  ; 
]  ; 
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>  if 


acceleraf  «  [  obsfil, 'a* ,int28tr(obstrk)  ]  ; 
eval(  [  'load  data\' ,positionf  ]) 
eval(  t  'load  data\' , velocity f  ]) 
eval(  [  'load  data\' , acceleraf  ]) 

% 

save  data\traclcn\ua.dat  obstrk  /ascii 
save  data\siinulpos  x  /ascii 

save  data\8imulvel  v  /ascii 

save  data\8imulacc  a  /ascii 

Z 
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D.  MI88ILE1.M 


MISSILEl.M 

This  program  was  developed  for  tracking  a  target  moving  at 
approximately  mock  three.  The  problem  is  set  up  for  the  poler 
coordinate 'system . 

dtl*0.01;' 
kmax-100 ; 

%Missile 

speed»l;  %approx.  3.3  time  the  speed  of  sound 

xrange»0;  %km 

yrange«20;  %km 

x»zeros ( 4 , kmax) ; 

x(: / l)=[xrange  3*speed/5  yrange  -4*speed/5] ’ ; 
theta«zeros(l,kmax) ; 
theta(l, l)=atan2 (x(l, 1) ,x(3, 1) ) ; 
t ime= zeros (1, kmax) ; 

A=[0  1  0  0;0  0  0  0;0  0  0  1;0  000]; 

B=[0  0;l  0;0  0;0  1]; 
tphi,del]=c2d(A,B,dtl) ; 

%Filter 

R=0.00i; 

Q»0; 

A=[0  1  0;0  0  l;0  0  0]; 

B=t0  01]'; 

H*[l  00]; 

[Phi,Del]=c2d(A,B,dtl) ; 

G^zeros ( 3 , kmax) ; 

XKKszeros ( 3 , kmax) ; 

XKKMl»zeros ( 3 , kmax) ; 

PKKMl»eye ( 3 ) *le7 ; 
for  kal:kmax 

G(: ,k)=PKKMl*H'*(H*PKKMl*H'+R)^(-l) ; 

PKK= (eye ( 3 ) -G ( ; , k) *H) *PKKM1 ; 

PKKMl=Phi*PKK*Phi ' +Q ; 

XKK(; ,k)«XKKMl(; ,k)+G(: ,k)*(theta(k)-XKKMl(l,k) ) ; 

XKKM1(; ,k+l)»Phi*XKK(; ,k) ; 
x( : ,k+l)*phi*x(; ,k) ; 
theta (k+l)=atan2(x(l,k+l) ,x(3,k+l) ) ; 
time (k+1) =time (k) +dtl ; 

end 

plot(x(l, :) ,x(3, :) ) ,grid,ylabel(' (Y  in  (km))') 
title ( 'Targets  track' ) ,xlabel (• (X  in  (km))’) 
meta  testl 

plot (time, theta*180/pi ), grid, title ( 'Bearing  to  the' target') 
xlabel ( ' (Sec) ' ) ,y label ( 'Degrees ' ) 
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neta  test2 

plot  (time  ( 1,  l:kmax)  ,XKK(1, : )  *180/pi)  ,grlcl 
title (' Kalman  filter  estimate  of  bearing') 
xlabel ( ' (Sec) ' ) , ylabel ( ' Degrees ' ) 
meta  test3 
look»10 ; 

plot  (time  (l,l:loolc)  ,6(l,l:look) )  ,title(  'G(l, :) ') 
xlabel ( ' (Sec) ' ) 
meta  tests 

plot ( time (1, 1: look) ,G(2, l:look) ) , title ( 'G(2, : ) ' ) 
xlabel ( ' (Sec) ' ) 
meta  test? 

plot (time (1,1: look) , G ( 3 , 1 : look) ) , title ( ' G ( 3 , : ) ' ) 
xlabel ( ' (Sec) ' ) 
meta  tests 

plot (XKK (2 ,:)) ,title( 'Estimate  of  Angular  rate') 
xlabel ( ' (Samples  taken) ' ) 
meta  test4 

plot (XKK ( 3, :)) /title ( 'Estimate  of  angular  acceleration') 
xlabel ( ' (Samples  taken) ' ) 
meta  tests 
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B.  11Z88ILB2.M 


This  program  was  developed  for  tracking  a  target  using  fusion. 
The  two  sensors  are  located  at  the  origin. 

dtl=0.01; 

dt2»l; 

kmax»3000; 

%Missile 

speed^l;  %approx.  3.3  time  the  speed  of  sound 

xrange»»0 ;  %km 

yrange»20;  %km 

x( : , l)«[xrange  3*speed/5  yrange  -4 * speed/5] ' ; 
theta (1, :)»atan2(x(l,l) ,x(3,l) ) ; 

A*[0  1  0  0;0  0  0  0;0  0  0  l;0  000]; 

B=[0  0;l  0;0  0;0  1]; 

[phi,del]=c2d(A,B,dtl) ; 

%Filter 

i=l; 

R-.Ol; 

Q=0; 

A*[0  1  0;0  0  l;0  0  0]; 

B»[0  0  1] • ; 

H*[l  0  0]; 

CPhi,Del]=c2d(A,B,dtl) ; 

XKKMl=zeros(3,l) ; 

PKKMl=eye(3)*le7; 

Phi_IR*eye(3) ; 
count =0 ; 
for  k=l:kmax 

G(: ,k)=PKKMl*H'*(H*PKKMl*H'+R)^(-l) ; 

PKK= ( eye ( 3 ) -G ( : , k) *H) *PKKM1 ; 

PKKMl=Phi*PKK*Phi ' +Q ; 

XKK(: ,k)=XKKMl(; ,k)+G(; ,k)* (theta {k)-XKKMl(l,k) ) ; 
if  count* 100+1  ==  k 

G(: ,i)=PKKMl*H'*(H*PKKMl*H'+R)^(-l) ; 

PKK= (eye ( 3 ) -G ( ; , i) *H) *PKKM1 ; 

PKKMl=Phi_IR*PKK*Phi_IR • +Q ; 

XKKM1(: ,i)»XKK(: ,k) ; 

XKK(: ,i)*XKKMl(: ,i)+G(: ,i)*(theta(k)-XKKMl(l,i) ) ; 
XKKM1(; ,k+l)=Phi*XKK(;,i) ; 
count»count+l ; 
i»i+l; 
else 

XKKMl(;,k+l)=Phi*XKK(:,k)  ; 

end 

x( ; ,k+l)*phi*x( : ,k) ; 

theta (k+l)satan2 (x(l,k+l) ,x(3,k+l) ) ; 
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end 

subplot (221) 

plot(x(l,;),x(3,:)),grid,ylabel('(Y  in  (km))') 
title ( 'Hlssiles  track' ) ,xlabel( ' (X  in  (km))') 
plot(theta*180/pi) , grid, title( 'Bearing  to  the  missile') 
xlabel ( ' Samples  taken  (t*. 01) • ) ,ylabel ( • Degrees ' ) 
plot(XKK(l, ;)*l80/pi) ,grid 
title (' Estimate  (For  Radar)') 

xlabel ('Samples  taken  (t=. 01) •) ,ylabel( 'Degrees' ) 
for  k»l:500 
t(k)»'dtl*k; 

end 

plot(t, G(l, 1:500) ) ,title('G(l,:) •), pause 
plot(t, G(2, 1:500)) ,title('G(2,:) ') 
plot(t, G(3, 1:500)) ,title('G(3,:) •) 
plot(XKK(2, :)) , title ( 'Angular  velocity') 
plot(XKK(3, :)) , title ( 'Angular  acceleration') 


41 


F.  8  BLLZP8B.M 


This  program  will  produce  error  ellipsoids  for  the  first 
few  samples  of  the  track. 

echo  off 
!del  pic3.met 

load  c : \matlab\wiseman\output2 . dat 
load  c : \matlab\wiseman\truepos2 . dat 
load  c : \matlab\wiseman\pkkout . dat 
r»9; 

axis ( ' square ' ) 
axis([-50  500  -50  500]) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  plot  path  and  estimate  of  path  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
plot (output2 (l:r, 1) ,output2 (l:r,2) , ’+' , 

truepos2 (l:r,2) , truepos2 (l:r, 3) ) 
xlabel('X  -  Position  (ft) • ) ;ylabel( ‘Y  -  Position  (ft)') 
title ( 'Trajectory  -  S  Curve') 

gtext(' _  True  track') 

gtext ( ' +  +  +  Estimate ' ) 
gtext('0  O  Error  Ellipsoids') 
gtext ( ' Enlarged  10  times ' ) 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  generate  a  uniformly  spaced  set  of  points  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
delt=»-l:  .05:1; 
deltspi*delt; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  generate  a  (2xm)  set  of  vectors  on  the  unit  circle  % 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
y«[cos(delt) ;sin(delt) ] ; 

shift«dlag(ones(41) ) ;  %  41  points  in  delt 

shift«shift' ; 
hold  on 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%  The  function  chol,  to  compute  the  Choleski  % 

%  decomposition,  is  done  "backwards"  in  MATLAB.  It  % 

%  makes  an  upper  triangular  matrix  R,  not  L.  So  take  % 

%  the  transpose.  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
for  k»l:9 
i=k-l ; 

P»pkkout(2*i+l;2*i+2,l:2) ; 

8«chol (P) ' ; 
x-s*y; 

plot ( X ( 1 , : ) * 10+output 2 ( k , 1 ) *shi f t , 

x(2, : ) *10+output2 (k, 2) *shift) 

end 
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%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%  An  array  of  x-vectors  (2xin)  has  been  created  that  % 
%  form  the  error  ellipsoid.  % 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
hold  off 
pause 
meta  plc3 
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G.  TORP8CUR.N 


This  is  the  program  to  generate  a  true  track  and  a  monte 
carlo  track.  The  program  generates  a  simulation  for  a  40  knot 
torpedo  following  an  s-shaped  track. 

Velocity  is  in  yards/second  and  position  is  in  feet.  Data 
orientation  is  similar  to  actual  data. 


ndim  -  =3; 


% 

% 

%%%%%  Establish  Measurement 
% 

sigmapos  =  diag([  15.0  , 

sigmavel  =  diag([  0.01  , 

sigmaacc  =  diag([  0.03  , 

% 

%%  Let  User  Input  Number  of 
%%  and  the  Seed  for  the 

% 


Standard  Deviations  %%%%%%%%% 

15.0  ,  0.00  ])  ; 

0.01  ,  0.00  ])  ; 

0.03  ,  0.00  ])  ; 

"Noisy"  Tracks  to  Generate  %% 
Random  Number  Generator  %% 


% 


nsimul  =  input ([' Enter  the  Base  Reference  Number  for 
this  Simulation  (100  <=  n  <=  900):  '])  ; 

nsimul  =  fix(min(max(nsimul, 100) ,900)  )  ; 
fprintf('  Your  Base  Reference  Number  is  -  %4.0f\n', 
nsimul)  ; 

ntrack  -  =  input ( • Enter  Number  of  Random  Tracks  to 

Generate:  ')  ; 


nseed  =  0  ; 

if  ntrack  >  0  ;  *  . 

nseed  =  input ('Enter  Seed  for  Random  Number 

Generator:  '); 

end 


% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


%%%%  The  Following  Section  Generates  the  "True"  %%%%% 
%%%%  and  Simulated  Tracks.  %%%%% 
%%%%  This  Section  Must  Be  Changed  as  Appropriate  %%%%% 
%%%%  in  Order  to  Generate  Other  Tracks  %%%%% 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

%%%%%%%%% 

%%%%%%%%%%%%%  Establish  Model  Constants  %%%%%%%%%%%%%%% 

% 


r  =  250; 

deltat  =  0.5; 

tfinal  =  45.0; 

kmax  =  tfinal/deltat  +  1; 

ydconv  =  3.0; 

v40kt  =  200/9  ; 

thetadot  =  ydconv*v40kt/r  ; 

% 
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% 

%%%%%%%%%%%%%  Initialize  Data  Arrays  %%%%%%%%%%%%%%% 

% 

X  -  zeros (kmax, ndim) ; 

V  s  zeros (kmax, ndim) ; 

a  =  zeros (kmax, ndim) ; 

% 

%%%%%  Generate  Accelerations  for  a  S-Track 

% 

v(l,l:2)  =  [  30  30  ]  ; 

theta  =  thetadot*deltat*(  (0:119  ) '+0.5)  ; 
a(25:34,l)  =  -12.*ones(10,l) ; 

a(45:54,2)  =  -12.*ones(10,l) ; 

a(65:74,l:2)  *  ones(10,l)*(  12.  12.  ]  ; 

% 

% 

%%%%%  Then  Generate  Velocities  and  Positions  by 
%%%%%  Piecewise  Constant  Integration 

% 

% 

for  i=2:kmax  ; 

v(i,:)  =  v(i-l,:)  +  a(i-l, : ) *deltat/ydconv  ; 

x(i,;)  =  x(i-l,:)  +  ydconv*v(i“l, : ) *deltat  ...  ; 

+  a(i-l, :)*deltat^2/2  ; 

end 

% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%%%%%  Plot  the  "True"  Track  %%%%% 

% 

eval(  [  '!  del  data\track* ,num2str(nsimul) , ' .met *  ]) 
plot  (x^: kmax,  1)  ,x(l:kmax,2) )  ;xlabel  ( 'x-axis' )  ; 
ylabel ( ' y-axis ' ) ; grid ; 

eval(  [  'meta  data\track' ,num2str(nsimul)  ]) 

% 

%  and  Save  the  True  Track  Position/Velocity/Acceleration  %% 
% 

eval(  [  'save  data\delt' ,num2str (nsimul) , '  deltat  '  ]) 
eval(  [  'save  data\truep' ,num2str (nsimul) , '  x  '  ]) 

eval(  [  'save  data\truev' ,num2str( nsimul) , '  v  '  ]) 

eval(  [  'save  data\truea' ,num2str( nsimul) , '  a  '  ]) 

% 

%%%%%  Next  Generate  the  Requested  Number  of  "Noisy"  %%%%% 


%%%%%  Tracks  by  Adding  Random  Noise  with  the  %%%%% 
%%%%%  Previously  Specified  Standard  Deviations  to  %%%%% 
%%%%%  the  "True"  Track.  (This  is  done  is  a  loop,  %%%%% 
%%%%%  with  each  track  graphed  and  saved  as  soon  as  %%%%% 
%%%%%  it  is  generated.)  %%%%% 
% 

truex  =  X  ;  truev  =  v  ;  truea  =  a  ; 
rand ( ' normal ' ) ; 


%%%% 


%%%%% 

%%%%% 
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% 

if  ntrack  >  0  ; 

rand ( ' seed ' , nseed) ; 

eval(  [  'save  data\seed' ,num2str(nslmul) , '  nseed  '  ]} 
for  j*l: ntrack  ;  %  Generate  Tracks 

X  =  truex  +  rand(kiaax,3)  *sigmapos  ; 

V  «  truev  +  rand(kinax,3)*sigmavel  ; 
a  =  truea  +  rand(kniax,3)  *siginaacc  ; 

%  %  Then  Save  them,  using 

%  %  the  'eval'  function 

eval(  [  'save  data\tstp' ,num2str(nsimul+j) , '  x  '  ]) 

eval(  [  'save  data\tstv' ,num2str(nsimul+j) , '  v  '  ]) 

eval(  [  'save  data\tsta' ,num2str(nsimul+j) , '  a  '  ]) 

% 

clg  ;  %  Then  Plot  Them 

eval(  [  '!  del  data\track' ,num2str(nsimul+j) , ' .met'  ]) 

plot (truex (l:kmax, 1) ,truex(l:kmax,2) ,x(l:]caax, 1) ,x(l:kmax,2) 
,' — '),title('x  (-)  and  xrandom  ( — )')  ; 
xlabel ( ' x-axis ' ) , ylabel ( ' y-axis ' ) , grid 

eval(  [  'meta  data\track' ,num2str (nsimul+j )  ]) 
end  ;  end 

% 


46 


H.  TRACK8XM.M 


% 

%  TRACKS IM.M 

%  THIS  PROGRAM  GENERATES  SIMULATED  TORPEDO  TRACKS  FOR 

%  TESTING  THE  KEYPORT  KAIKAN  FILTERING.  THIS  PROGRAM  AS 
%  CURRENTLY  WRITTEN  GENERATES  A  SIMULATION  FOR  A  40  KNOT 
%  TORPEDO  ENTERING  A  250  FT  RADIUS  CIRCLE  AT  A  TURNING  RATE 
%  OF  15  DEGREES/SECOND.  THE  VELOCITY  IS  IN  YARDS/SECOND 
%  AND  POSITION  IS  IN  FEET,  AND  THE  DATA  ORIENTATION  IS 
%  SIMILAR  TO  ACTUAL  DATA.  HOWEVER,  THE  PROGRAM  IS  DESIGNED 

%  SO  THAT  ONE  SIMPLY  NEED  CHANGE  THE  ACCELERATION  DATA  (IN 

%  THE  PORTION  OF  THE  CODE  WHERE  INDICATED) 

% 

% 

ndim  *  3 ; 

% 

% 

%%%%%%  Establish  Measurement  Standard  Deviations  %%%%%%%%% 
% 

sigmapos  =  diag([  15.0  ,  15.0  ,  0.00  ])  ; 

sigmavel  =  diag([  0.01  ,  0.01  ,  0.00  ])  ; 

sigmaacc  =  diag([  0.03  ,  0.03  ,  0.00  ])  ; 

% 

%%%  Let  User  Input  Number  of  "Noisy”  Tracks  to  Generate  %%% 
%%%  and  the  Seed  for  the  Random  Number  Generator  %%% 

% 

nsimul  ==  input  ([' Enter  the  Base  Reference  Number  for 

this  Simulation  (100  <=  n  <=  900):  '])  ; 

nsimul  =  f ix (min (max (nsimul, 100) , 900)  )  ; 

fprintf ( ' Your  Base  Reference  Number  is  -  %4. Of \n nsimul) ; 

% 

ntrack  =  input ( ' Enter  Number  of  Random  Tracks  to 

Generate:  ')  ; 

nseed  =  0  ; 

if  ntrack  >  0  ; 

nseed  =  input ( ' Enter  Seed  for  Random  Number 

Generator:  ' ) ; 

end 

% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 


%%%  The  Following  Section  Generates  the  "True"  %%% 
%%%  and  Simulated  Tracks.  %%% 
%%%  This  Section  Must  Be  Changed  as  Appropriate  %%% 
%%%  in  Order  to  Generate  Other  Tracks  %%% 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%%%%%%%%%%%%%%  Establish  Model  Constants  %%%%%%%%%%%%%%% 
% 

r  =  250; 
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deltat  »  0.5; 

tfinal  =  80.0; 

kmax  »  tfinal/deltat  -t-  1; 

ydconv  =  3.0; 

v40)ct  =  200/9  ; 

thetadot  =  ydconv*v40kt/r  ; 

%%%%%%%%%%%%%%  Initialize  Data  Arrays  %%%%%%%%%%%%%% 

% 

X  s  zeros (kmax, nd in ) ; 

V  »  zeros (kmax, ndim) ; 

a  »  zeros (kmax, ndim) ; 

% 

% 

%%%%  Generate  Accelerations  for  a  Circular  Track  %%%%% 
% 

v(l,;)  *  [  v40kt  ,0,0]; 

theta  =  thetadot*deltat* (  (0:119  ) '+0.5)  ; 

a(20,l:2)  =  [  -v40kt/deltat  ,  v40kt/deltat  ] 

♦ydconv  ; 

a(21:140,l:2)  =  (  (v40kt*ydconv) ^2/r  ) 

*[  -cos (theta)  ,  -sin (theta)  ]  ; 

% 

% 

%%%%%  Then  Generate  Velocities  and  Positions  by  %%%%%%%%% 
%%%%%  Piecewise  Constant  Integration  %%%%%%%%% 

% 

% 

for  i=2;kmax  ; 

v(i,;)  =  v(i-l,;)  +  a(i-l, : ) *deltat/ydconv  ; 

x(i,;)  =  x(i-l,;)  +  ydconv*v(i-l, : ) *deltat  ...  ; 

+  a(i-l, : ) *deltat^2/2  ; 
end  ; 

% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% 

%%%%%%  Plot  the  "True"  Track  %%%%%%%%%%% 

% 

eval(  [  '!  del  data\track' ,num2str (nsimul) , ' .met '  ]) 
plot(x(l:kmax,l) ,x(l:kmax,2) ) ;xlabel ( 'x-axis* ) ; 
ylabel ( ' y-axis ' ) ; grid ; 

eval(  [  'meta  data\track' ,num2str (nsimul)  ]) 

% 

%  and  Save  the  True  Track  Position/Velocity/Acceleration  %% 

% 

eval(  [  'save  data\delt ' ,num2str (nsimul) , '  deltat  '  ]) 
eval(  (  'save  data\truep' ,num2str (nsimul) , '  x  '  ]) 

eval(  [  'save  data\truev' ,num2str( nsimul) , '  v  '  ]) 

eval(  [  'save  data\truea' ,num2str( nsimul) , '  a  '  ]) 

% 

%%%%%  Next  Generate  the  Requested  Number  of  "Noisy"  %%%%% 
%%%%%  Tracks  by  Adding  Random  Noise  with  the  %%%%% 
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%%%%%  Previously  Specified  Standard  Deviations  to  %%%%% 
%%%%%  the  "True"  Track.  (This  is  done  is  a  loop,  %%%%% 

%%%%%  with  each  track  graphed  and  saved  as  soon  as  %%%%% 
%%%%%  it  is  generated.)  %%%%% 

% 

truex  “  X  ;  truev  *  v  ;  truea  *  a  ; 
rand ( ' normal ' ) ; 

% 


if  ntrack  >  0  ; 

rand ( ' seed ' , nseed) ; 

eval(  [  'save  data\seed' ,num2str (nsimul) , '  nseed  '  ]) 
for  j=l: ntrack  ;  %  Generate  Tracks 

X  -  truex  +  rand(kmax,3)*sigmapos  ; 

V  *  truev  +  rand(kmax,3)*sigmavel  ; 
a  =  truea  +  rand(kmax,3) *sigmaacc  ; 

%  %  Then  Save 

them,  using 

%  %  the  'eval' 

function 

eval(  [  'save  data\tstp' ,num2str (nsimul+j ) , '  x  '  ]) 

eval(  [  'save  data\tstv' ,num2str (nsimul+j) , '  v  '  ]) 

eval(  [  'save  data\tsta' ,num2str (nsimul+j ), '  a  '  ]) 

% 


% 


clg  ;  %  Then  Plot  Them 

eval(  [  '!  del  data\track' ,num2str( nsimul+j) ,' .met'  ]) 


plot (truex ( 1 :kmax, 1) ,truex(l:kmax,2) ,x(l:kmax,l) ,x(l:kmax,2) 

title('x  (-)  and  xrandom  ( — )')  ; 
xlabel ( ' x-axis ' ) , ylabel ( ' y-axis ' ) , grid 

eval(  [  'meta  data\track' ,num2str( nsimul+j )  ]) 
end  ;  end 

% 
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APPENDIX  C:  THE  KALMAN  FILTER  AND  MANEUVER  DETECTION 


A.  0B8DATA.F0R 

C  OBSDATA.FOR 

C  MODIFICATIONS  MADE  BY  ART  SCHOENSTADT  ON  AUGUST  14,  1992. 
C 

C - THIS  PROGRAM  IS  DESIGNED  TO  PROCESS  3/D  DATA  FILES  WHICH 

C - SIMULATE  THE  UNDERSEA  TRACKING  RANGE  AT  KEYPORT,  WA. 

C— THE  INPUT  DATA  FILES  ARE  GENERATED  BY  A  MATLAB  SIMULATION. 

C - THIS  PROGRAM  IS  DESIGNED  TO  RUN  ON  THE  IBM/AT  PERSONAL 

C— COMPUTER.  THE  USER  IS  ALSO  REQUIRED  TO  PROVIDE  THE  INPUT 

C - DATA  FILES  IN  SPECIFIC  FORMAT,  AND  THE  NUMBER  OF  POINTS  TO 

C— CALCULATE  (MAXIMUM  OF  2500)  .  THE  PROGRAM  USES  A 

C - LARGE  AMOUNT  OF  HARD  DISK  SPACE  FOR  TEMPORARY 

C - FILES  (APPROXIMATELY  160K  BYTES/100  POINTS,  4MEG  BYTES  MAX 

C - FOR  2500  POINTS. 

C— THE  PROGRAM  ALSO  INCLUDES  "HARD  WIRED”  CONVERSIONS  TO 

C - REFLECT  THE  FACT  THAT  SOME  DATA  IS  IN  FEET,  OTHER  DATA  IN 

C - YARDS,  AND  DEPTH  IS  MEASURED  IN  POSITIVE,  NOT  NEGATIVE 

C— TERMS . 

C 

C****************  SECTION  1  ************************** 


C 

C***  DECLARATION  OF  PARAMETERS  *** 

C***  MAXOBS  IS  THE  MAXIMUM  NUMBER  OF  OBSERVATIONS  THAT  *** 
C***  CAN  BE  USED  *** 

C***  MAXSTATE  IS  SET  FOR  A  3 -DIMENSIONAL,  POSITION,  *** 

C***  VELOCITY  AND  ACCELERATION  PLANT  *** 

C***  DEPS  IS  USED  TO  TEST  WHEN  TWO  OBSERVATIONS  ARE  CLOSE  *** 
C***  ENOUGH  TO  BE  CONSIDERED  SIMULTANEOUS  *** 


C 

PARAMETER (  MAXOBS=250,  MAXSTATE=9  ) 

PARAMETER (  DEPS=0. 005D0,  STEP=0.5D0  ) 

C 

C  *****  DECLARATION  OF  VARIABLES  ***** 

C 

DOUBLE  PRECISION  PTC (MAXOBS) ,VTIME( MAXOBS ) , 

*  SOURCE (MAXOBS ) , ZK (MAXSTATE , MAXOBS ) , ROLL(MAXOBS ) 
DOUBLE  PRECISION  SS,SSV,SEC,X,Y,Z,XVEL, 

*  YVEL , Z VEL , ACCX , ACCY , ACCZ , E YAW , EROLL , EPITCH , PTM , VTM , 

*  ATM , RTM , ATMl , ACCXMl , ACCYMl , ACCZMl , RLLMl , SSA , SSR , 

*  TORANGE , TOTORP , TNEXTOBS 

C  ^ 

INTEGER  HH , HHV , MM , MMV , HOUR , MIN , PC , MMA , MMR , NOBSERV , SRC 
C 
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CHARACTER  ASK*1 

CHARACTER  PKKS3D*13,  PKKM1S*13,  INFILC*13,  INFILR*13 
CHARACTER  INFILP*13,  INFILA*13,  OUTFIL*13 


LOGICAL* 1  POSREC , VELREC , ACCREC , ENDPOSDAT , ENDVELDAT , 
*  ENDACCDAT , ENDRLLDAT 


****  INPUT  DATA,  DESIGNATE  FILENAMES  **** 


OPEN(l,  FILE* 
OPEN (2,  FILE* 
OPEN (3,  FILE* 
OPEN (4,  FILE* 
OPEN (11, FILE* 


•  DATA\POSITION . OBS ' ,  STATUS* 
'  DATA\VELOCITY . OBS • ,  STATUS* 

•  DATA\ACCELERA . OBS • ,  STATUS* 
'  DATA\ROLL_YAW . OBS ' ,  STATUS* 
'  DATA\OBSERVAT . DAT • ,  STATUS* 


•OLD') 

•OLD') 

•OLD') 

•OLD') 

•NEW) 


****  NOTIFY  USER  OF  MAXIMUM  NUMBER  OF  DATA  POINTS  **** 


WRITE (*,770)  MAXOBS 

770  FORMAT ('  The  MAXIMUM  number  of  points  that  can  be  ', 

*  'analyzed  is: ',14) 

WRITE(*,771) 

771  FORMAT ('  To  use  more,  you  must  reset  the  parameter  ', 

*  'MAXOBS',/,'  in  the  main  program  and  recompile') 


ithhitititifkitititit**  SECTION  2  ************************** 

****  get  starting  time  for  range  data  **** 

READ (1, 750, END=1200)  PC,X, Y, Z ,HH,MM, SS 
WRITE(*,751)  HH,MM,SS 

TORANGE  =  3600.*REAL(HH)+60.*REAL(MM)+SS 
WRITE (*,752) 

READ(*,755)  HOUR, ASK, MIN, SEC 
IF  (ASK  .EQ.  '  ')  GOTO  754 

TORANGE  *  3600,*REAL(HOUR)+60.*REAL(MIN)+SEC 
WRITE (*,756)  HOUR, MIN, SEC 

754  CONTINUE 
C 

750  FORMAT(1X,I6,2X,3F10.1,10X,I2,1X,I2,1X,F5.2) 

751  FORMAT (/, '  The  first  position  time 
*is: ' ,12, • : • ,12, ' ; ' ,F7.4) 

752  FORMAT(/, '  If  you  do  not  wish  to  change  it,  press 

* "Enter '  If  you  wish  to  change  it,  enter  the  new 
*time  in  the  format:  ',/,'  HH:MM:SS.SSSS ' ) 

755  F0RMAT(I2,A1,I2,1X,F7.4) 

756  FORMAT('  The  first  position  time  is 
*now: ' ,12, ' : ' ,12, • : ' ,F7.4) 
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****  get  starting  time  for  torpedo  data  **** 

READ(2, 760, END-1201)  MMV,SSV,XVEL,YVEL,ZVEL 
HHV  -  0 

TOTORP  *  60.*REAL(MMV)+SSV 
WRITE(*,761)  HHV,MMV,SSV 
WRITE (*,752) 

READ(*, 755, END-763)  HOUR, ASK, MIN, SEC 
IF  (ASK  .EQ.  •  •)  GOTO  763 

TOTORP  -  3600.*REAL(HOUR)+60.*REAL(MIN)+SEC 
WRITE (*,762)  HOUR, MIN, SEC 
763  CONTINUE 
C 

760  FORMAT ( IX , 12 , IX , F7 . 4 , lOX, F9 . 3 , 5X , F9 . 3 , 5X , F9 . 3 ) 

761  FORMAT (/,'  The  first  velocity  time 
*is: ' ,12, • : • ,12, ' : ' ,F7.4) 

762  FORMATC  The  first  velocity  time  is 
*now: • ,12, ' : ' ,12, • ; ' ,F7.4) 

C 

C 

C  ****  SELECT  THE  FIRST  RECORD  FROM  EACH  DATA  SET  THAT  **** 
C  ****  REPRESENTS  AN  OBSERVATION  AT  OR  AFTER  THE  STARTING  **** 


C  ****  TIME.  NOTE  THAT  WE  MUST  USE  A  SLIGHTLY  DIFFERENT  **** 
C  ****  PROCEDURE  FOR  THE  POSITION  AND  VELOCITY  DATA  **** 
C  ****  FILES,  SINCE  WE  HAVE  ALREADY  READ  A  RECORD  FROM  **** 

c  ****  each  of  them.  **** 


201  CONTINUE 

IF  (  PTM  .GE.  TORANGE  )  GOTO  202 
READ( 1,750, END-1200)  PC,X, Y, Z,HH,MM, SS 
PTM  =  3600.*REAL(HH)+60.*REAL(MM)+SS 
GOTO  201 

202  CONTINUE 

IF  (  VTM  .GE.  TOTORP  )  GOTO  203 
READ (2, 760, END-1201)  MMV,SSV, XVEL, YVEL, ZVEL 
VTM  =  60.*REAL(MMV)+SSV 
GOTO  202 

203  CONTINUE 

READ (3, 760, END-1202)  MMA,SSA, ACCX, ACCY,ACCZ 
ATM  -  60.*REAL(MMA)+SSA 
IF  (  ATM  .LT.  TOTORP  )  GOTO  203 
ATMl-ATM 
ACCXMl-ACCX 
ACCYMl-ACCY 
ACCZMl-ACCZ 

204  CONTINUE 

READ (4,760, END-1203 )  MMR, SSR, EYAW, EROLL, EPITCH 
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RTM  »  60.*REAL(MMR)-fSSR 
IF  (  RTM  .LT.  TOTORP  )  GOTO  204 
RTMl-RTM 
RLI2fl»EROLL 


C 

C 

c  **** 
c 

PTM 

VTM 

ATM 

RTM 

C 


SYNCHRONIZE  RANGE 

PTM  -  TORANGE 
VTM  -  TOTORP 
ATM  -  TOTORP 
RTM  -  TOTORP 


C****************  SECTION  3 


AND  TORPEDO  CLOCKS  ****** 


************************* 


C 

c***  BUILD  THE  OBSERVATION  ARRAY  BY  SEQUENTIALLY  *** 

C***  DETERMINING  THE  NEXT  OBSERVATION  TIME  AND  THE  *** 

C***  SPECIFIC  FIELDS  OBSERVED  AT  THAT  TIME  *** 

C 
C 

WRITE(*,*)  '  ' 

WRITE (*,*)  'Reading  Data  Files' 

NOBSERV  »  0 
ENDPOSDAT  =  .FALSE. 

ENDVELDAT  =  .FALSE. 

ENDACCDAT  =  .FALSE. 

ENDRLLDAT  =  .FALSE. 

C 

300  CONTINUE 

IF  (ENDVELDAT  .AND.  ENDPOSDAT  )  GOTO  400 
IF  ( NOBSERV. GE.MAXOBS)  GOTO  395 

C 

NOBSERV  =  NOBSERV  +  1 
POSREC  =  .FALSE. 

VELREC  =  .FALSE. 

ACCREC  =  .FALSE. 

TNEXTOBS  *  DMIN1(PTM,VTM) 

IF  (ENDPOSDAT)  TNEXTOBS  =  VTM 

IF  (ENDVELDAT)  TNEXTOBS  =  PTM 

VTIME (NOBSERV)  =  TNEXTOBS 
C 

C***  SEE  IF  THERE'S  A  POSITION  OBSERVATION  AT  THIS  TIME  *** 
C 

311  CONTINUE 

IF  (  (  (PTM-TNEXTOBS)  .LT.  DEPS  )  .AND. 

*  (  .NOT. (ENDPOSDAT)  )  )  THEN 

POSREC  B  .TRUE. 

PTC (NOBSERV) »PC 
ZK (1, NOBSERV) bX 
ZK( 4, NOBSERV) *Y 
ZK( 7, NOBSERV) bZ 
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READ(1,750,END=312)  PC,X, Y, Z,HH,MM,SS 
PTM  =  3600.*REAL(HH)+60.*REAL(MM)+SS  -  TORANGE 
IF  (  PTM  .LT.  TNEXTOBS  )  GOTO  1210 
ENDIF 
GOTO  321 
C 

312  ENDPOSDAT=.TRUE. 

WRITE (*,313)  NOBSERV 

313  FORMAT ('  End  of  Position  Record  File  Encountered  at 

*  'Observation  ',14) 

C 

C 

C***  SEE  IF  THERE'S  A  VELOCITY  OBSERVATION  AT  THIS  TIME  * 
C 

321  CONTINUE 

IF  (  (  (VTM-TNEXTOBS)  .LT.  DEPS  )  .AND. 

*  (  .NOT. (ENDVELDAT)  )  )  THEN 
VELREC  =  .TRUE. 

ZK( 2, NOBSERV) =  3.*XVEL 
ZK ( 5 , NOBSERV) =-3 . *YVEL 
ZK ( 8 , NOBSERV) =-3 . *ZVEL 

READ(2,760,END=322)  MMV,SSV,XVEL, YVEL, ZVEL 
VTM  =  60.*REAL(MMV)+SSV  -  TOTORP 
IF  (  VTM  .LT.  TNEXTOBS  )  GOTO  1211 
ENDIF 
GOTO  331 
C 

322  ENDVELDAT*. TRUE. 

WRITE (*,323)  NOBSERV 

323  FORMAT ('  End  of  Velocity  Record  File  Encountered  at 

*  ' Observat ion  ',14) 

C 

C 

C*  SEE  IF  THERE'S  AN  ACCELERATION  OBSERVATION  AT  THIS  TIME 
C*  THEN  USE  THE  CLOSEST  ACCELERATION  OBSERVATION  TO  THE 
C*  CURRENT  TIME. 

C 

331  CONTINUE 

IF  (  (  ATM  .LE.  TNEXTOBS  )  .AND. 

*  (  .NOT. (ENDACCDAT)  )  )  THEN 
ATM1=ATM 

ACCXM1*ACCX 

ACCYM1=ACCY 

ACCZM1*ACCZ 

332  READ(3,760,END=333)  MMA,SSA, ACCX, ACCY, ACCZ 
ATM  »  60.*REAL(MMA)+SSA  -  TOTORP 

IF  (  ATM  .LE.  TNEXTOBS  )  GOTO  332 
ENDIF 
GOTO  335 


333  ENDACCDAT*. TRUE 
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WRITE (*,334)  NOBSERV 

334  FORMATC  End  Of  Acceleration  Record  File  Encountered  at 
*Observatlon  *,14) 

c 

335  CONTINUE 

IF  (  (TNEXTOBS-ATMl)  .LT.  (ATM  -  TNEXTOBS)  )  THEN 
ZK ( 3 , NOBSERV) =ACCXM1 
ZK ( 6 , NOBSERV) =ACCYM1 
ZK ( 9 , NOBSERV) =ACCZM1 
ELSE 

ZK ( 3 , NOBSERV) *ACCX 
ZK ( 6 , NOBSERV) =ACCY 
ZK ( 9 , NOBSERV) =ACCZ 

ENDIF 

C 

C***  SEE  IF  THERE'S  A  ROLL  OBSERVATION  AT  THIS  TIME  ***** 
C***  THEN  USE  THE  CLOSEST  ROLL  OBSERVATION  TO  THE  ***** 
C***  CURRENT  TIME.  ***** 


341  CONTINUE 

IF  (  (  RTM  .LE.  TNEXTOBS  )  .AND. 

*  (  .NOT. (ENDRLLDAT)  )  )  THEN 
RTM1=RTM 

RLIMl=EROLL 

342  READ(4,760,END«343)  MMR,SSR,EyAW,EROLL,EPITCH 
RTM  »  60.*REAL(MMR)+SSR  -  TOTORP 

IF  (  RTM  .LT.  TNEXTOBS  )  GOTO  342 
ENDIF 
GOTO  345 

343  ENDRLLDAT=.TRUE. 

WRITE (*,344)  NOBSERV 

344  FORMATC  End  of  Roll  Record  File  Encountered  at  ', 

*  'Observation  ',14) 

C 

345  CONTINUE 

IF  (  (TNEXTOBS-RTMl)  .LT.  (RTM  -  TNEXTOBS)  )  THEN 
ROLL (NOBSERV) =RLLM1 
ELSE 

ROLL (NOBSERV) =EROLL 

ENDIF 

ACCREC  =  .TRUE. 

C 

C****  CODE  THE  SOURCE (S)  OF  THIS  RECORD  ****** 

C 

351  CONTINUE 
SRC  =  0 

IF  (  POSREC  )  SRC  «  73 

IF  (  VELREC  )  SRC  «  SRC  +146 

C  IF  (  ACCREC  )  SRC  =  SRC  +  292 

SOURCE (NOBSERV)*  SRC 


c 

c 

GOTO  300 
C 

395  WRITE (*,396)  NOBSERV,MAXOBS 

396  FORMAT ('  The  total  number  of  observations  (',14,') 
*exceeds  allocate  storage  (MAXOBS  =  ' ,I4,/,5x, 'Some  data 
♦will  be  lost.  You  may  wish  to  recompile  the  program',/, 

*  with  a  higher  value  of  MAXOBS.  (Enter  "Y"  to 
♦continue) ' ) 

READ(*,397)  ASK 

397  FORMAT (Al) 

IF  (  (  ASK  .NE.  'Y'  )  .AND.  (  ASK  .NE.  'y'  )  )  STOP 
C 
C 

C*****************  SECTION  4  ************************** 

€♦♦♦♦  WRITE  OUT  DATA  FILES  **** 

C 
C 

400  CONTINUE 
CLOSE(l) 

CLOSE (2) 

CLOSE (3) 

CLOSE (4) 

WRITE (♦,902)  NOBSERV 
WRITE (11, 901)  NOBSERV 
DO  401  IPRT  »  1, NOBSERV 

WRITE (11, 900)  PTC (IPRT) ,VTIME(IPRT) , SOURCE (IPRT) , 

♦  (  ZK(JPRT, IPRT) ,JPRT=1,MAXSTATE) , ROLL (IPRT) 

401  CONTINUE 

900  F0RMAT(13(1X,E14.8) ) 

902  format!/,'  Writing  ',15,'  Combined  Observations  to 
♦File' ) 

901  FORMAT ('  OUTPUT  FROM  PROGRAM  OBSDATA.FOR  ',/, 

♦IX, 14 , '  OBSERVATIONS ' , 

♦/,7X, 'PTC ,11X, 'TIME' ,10X, 'SOURCE ', lOX, 'ZK(l) ' ,10X, 
♦'ZK(2) ' ,10X, 'ZK(3) ' ,10X, 'ZK(4) ' ,10X, 'ZK(5) ' ,10X, 'ZK(6) ' , 
♦lOX, 'ZK(7) ' ,10X, 'ZK(8) ' ,10X, 'ZK(9) ' ,10X, 'ROLL') 

CLOSE (11) 

C 

C 

STOP 

C 

C  ****  ERROR  ROUTINES  FOR  UNEXPECTED  END  OF  DATA  FILE  ♦♦♦♦ 
C 

1200  WRITE(^,1300) 

1300  FORMATC^^^^  ERROR  -  RANGE  DATA  FILE  ENDED  BEFORE  A 
♦VALID  ', 'RECORD  COULD  BE  LOCATED') 

STOP 

C 

1201  WRITE(^,1301) 
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1301  FCl?!i«AT(  *****  ERROR  -  VELOCITY  DATA  FILE  ENDED  BEFORE  A 
* VALID  RECORD  COULD  BE  LOCATED’) 

STOP 


C 


1202  WRITE(*,1302) 

1302  FORMAT ('****  ERROR  -  ACCELERATION  DATA  FILE  ENDED  BEFORE 
*A  VALID  RECORD  COULD  BE  LOCATED*) 

STOP 

C 


1203  WRITE(*,1303) 

1303  FORMAT (’****  ERROR  -  ROLL  DATA  FILE  ENDED  BEFORE  A  VALID 
*RECORD  COULD  BE  LOCATED') 

STOP 


1210  WRITE(*,1310) 

1310  FORMAT (*****  ERROR  -  RANGE  DATA  FILE  RECORD  OUT  OF  VALID 
*TIME  ORDER') 

STOP 

C 

1211  WRITE(*,1311) 

1311  FORMAT (•****  ERROR  -  VELOCITY  DATA  FILE  OUT  OF  VALID 
*TIME  ORDER') 

STOP 

C 

1212  WRITE(*,1312) 

1312  FORMAT  ('****  ERROR  -  ACCELERATION  DATA  FILE  OUT  OF  VALID 
*TIME  ORDER') 

STOP 

C 

1213  WRITE(*,1313) 

1313  FORMAT ('****  ERROR  -  ROLL  DATA  FILE  OUT  OF  VALID 
*TIME  ORDER') 

STOP 

C 

END 
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C 

C  POSCONV.FOR 

C 

C  THIS  PRCKSRAM  CONVERTS  THE  DATA  FILES  CREATED  BY  THE 
C  MATLAB  SIMULATION  FOR  POSITION,  VELOCITY,  AND 
C  ACCELERATION  INTO  THE  PROPER  FORMAT  FOR  USE  BY  THE  KALMAN 
C  FILTER  PROGRAM.  IT  ALSO  CREATES  A  ROLL  ANGLE  DATA  FILE 
C  OF  0  DEGREES  THROUGHOUT. 

C 

INTEGER  PC, MIN, HR 

DOUBLE  PRECISION  SEC,X, Y,Z,VX,VY,VZ,AX,AY,AZ, YAW, 
DOUBLE  TRECISION  ROLL, PITCH , TOTSEC , DELTAT 
LOGICAL*!  ENDPOSREC,  ENDVELREC,  ENDACCREC 


*******  OPEN  THE  TIMING  INFORMATION  DATA  FILE  *********** 
OPEN ( 1 , FILE= ' DATA\DELTSIMU  * , STATUS= • OLD • ) 

*******  OPEN  THE  INPUT  POSITION  DATA  FILE  *********** 

OPEN ( 2 , FILE= • DATA\SIMULPOS ' , STATUS= ' OLD ' ) 

******  OPEN  THE  INPUT  VELOCITY  DATA  FILE  *********** 

OPEN ( 3 , FILE= ' DATA\SIMULVEL • , STATUS= • OLD ' ) 


******  OPEN  THE  INPUT  ACCELERATION  DATA  FILE  *********** 
OPEN ( 4 , FILE* • DATA\SIMULACC ' , STATUS* ' OLD ' ) 


******  OPEN  THE  OUTPUT  POSITION  DATA  FILE  *********** 

OPEN (12, FI LE= ' DATA\POS ITION. OBS STATUS* 'NEW' ) 

******  OPEN  THE  OUTPUT  VELOCITY  DATA  FILE  *********** 

OPEN ( 13 , FILE* ' DATA\VELOCITY . OBS ' , STATUS* ' NEW ' ) 

******  OPEN  THE  OUTPUT  ACCELERATION  DATA  FILE  *********** 
OPEN ( 14 , FILE* ' DATA\ACCELERA. OBS • , STATUS* • NEW ' ) 

******  OPEN  THE  OUTPUT  ROLL  ANGLE  DATA  FILE  *********** 
OPEN ( 15 , FILE* ' DATA\ROLL_YAW . OBS ' , STATUS* ' NEW ' ) 


*********************************************************** 
***  INPUT  THE  MATLAB  FILES  AND  CONVERT  TO  INPUT  **** 
***  FORMAT  FOR  THE  FILTER  PROGRAM  **** 

*********************************************************** 


PC=1 
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TOTSEC=  O.DO 
READ (1,20)  DELTAT 


ENDPOSREC  *  .FALSE. 

ENDVELREC  »  .FALSE. 

ENDACCREC  »  .FALSE. 

100  CONTINUE 

IF  (  ENDPOSREC  .AND.  ENDVELREC  .AND.  ENDACCREC  ) 
*  GOTO  200 

***  UPDATE  TIMING  DATA 

SEC  *  AMOD(TOTSEC,60.0) 

MIN  =  MOD(  DINT(TOTSEC/60.D0) ,  60  ) 

HR  =  DINT(TOTSEC/3600.D0) 

***  PROCESS  ROLL,  YAW  AND  PITCH  DATA 

YAW=.00 

ROLL=.00 

PITCH=.00 

WRITE (15,40)  MIN , SEC , YAW , ROLL , PITCH 

***  PROCESS  POSITION  DATA 

IF  (  .NOT.  ENDPOSREC  )  THEN 
READ(2,20,END*101)  X,Y,Z 
WRITE(12,30)  PC,X,Y,Z,HR,MIN,SEC 
ENDIF 

***  PROCESS  VELOCITY  DATA 

IF  (  .NOT.  ENDVELREC  )  THEN 
READ(3,20,END=102)  VX,VY,VZ 
VY=-VY 
VZ*-VZ 

WRITE(13,40)  MIN,SEC,VX,VY,VZ 
ENDIF 

***  PROCESS  ACCELERATION  DATA 

IF  (  .NOT.  ENDACCREC  )  THEN 
READ(4,20,END=103)  AX,AY,AZ 
WRITE (14, 40)  MIN, SEC, AX, AY, AZ 
ENDIF 

***  UPDATE  COUNTERS  AND  TIMING 

PC=PC+1 

TOTSEC=TOTSEC+DELTAT 


*** 


*** 


*** 


*** 


*** 
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nan 


c 

GOTO  100 


20  F0RMAT(3(1X,E15.7) ) 

30  FORMAT(1X,I6,2X,3F10.1,10X,I2,1X,I2,1X,F5.2) 

40  F0RMAT(1X,I2,1X,F7.4,10X,F9.3,5X,F9.3,5X,F9.3) 

C 

101  CONTINUE 

WRITE (*,121)  PC-1 
ENDPOSREC  =  .TRUE. 

GOTO  100 

121  FORMAT(  '  End  of  Data  After  ',14,'  Position  Records 
*Read ' ) 

C 

102  CONTINUE 

WRITE (*,122)  PC-1 
ENDVELREC  =  .TRUE. 

GOTO  100 

122  FORMAT(  '  End  of  Data  After  ',14,'  Velocity  Records 
*Read ' ) 

C 

103  CONTINUE 

WRITE (*,123)  PC-1 
ENDACCREC  =  .TRUE. 

GOTO  100 

123  FORMAT (  '  End  of  Data  After  ',14,'  Acceleration  Records 
*Read ' ) 

C 

200  CONTINUE 
CLOSE (1) 

CLOSE (2) 

CLOSE ( 3 ) 

CLOSE (4) 

CLOSE (12) 

CLOSE (13) 

CLOSE (14) 

CLOSE (15) 

STOP 

END 
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C.  TORPNAN.FOR 


C~KALMAN  FILTER - KAUIAN  SMOOTHING  ALGORITHM - 

C — THIS  PROGRAM  IS  DESIGNED  TO  PROCESS  3/D  DATA  FILES  FROM 
C — THE  UNDERSEA  TRACKING  RANGE  AT  KEYPORT  HA.  A  KALMAN 
C~FILTER  IS  APPLIED  TO  THE  TRACK  DATA  WHICH  CONSISTS  OF 
C“X,Y,  AND  Z  COORDINATES,  AS  WELL  AS  VELOCITY  COMPONENT  IN 
C— X,Y,  AND  Z  COORDINATES.  THEN,  A  KAIMAN  FILTER  SMOOTHING 
C— ROUTINE  GENERATES  SMOOTHED  POINTS  IN  X,Y,  AND  Z.  THE 
C— PROGRAM  GENERATES  OUTPUT  FILES  WHICH  CONTAIN  THE  VARIANCE 
C— OF  THE  X  ESTIMATE  VS  SAMPLE  FOR  BOTH  THE  FORWARD  KAIMAN 
C — FILTER  CASE  AND  THE  KAIMAN  SMOOTHED  CASE.  FILES  ARE  ALSO 
C — GENERATED  WHICH  CONTAIN  THE  FILTERED  X,Y,AND  Z  ESTIMATES 
C— AND  THE  SMOOTHED  X,Y,  AND  Z  ESTIMATES.  THIS  PROGRAM  IS 
C — DESIGNED  TO  RUN  ON  THE  IBM/ AT  PERSONAL  COMPUTER  BUT  DUE 
C — TO  THE  SIZE  OF  THE  DATA  SETS  INVOLVED,  PLOTTING  CANNOT  BE 
C — DONE  WITH  THIS  PROGRAM.  PLOTTING  OF  OUTPUT  DATA  IS 
C— DONE  USING  MATLAB.  THE  PROGRAM  GIVES  THE  USER  THE  OPTION 
C — OF  CHANGING  THE  STARTING  TIMES  FOR  BOTH  THE  RANGE  AND 
C— VELOCITY  DATA,  THE  VALUE  OF  THE  INITIAL  COVARIANCE 
C— MATRIX,  AND  THE  EXCITATION  PROCESS  VECTOR.  THE  USER  IS 
C— ALSO  REQUIRED  TO  PROVIDE  THE  NAMES  OF  THE  INPUT  AND 
C — OUTPUT  DATA  FILES,  AND  THE  NUMBER  OF  POINTS  TO  CALCULATE 
C— WITH  A  MAXIMUM  OF  2500.  THE  PROGRAM  USES  A  LARGE  AMOUNT 
C— OF  HARD  DISK  SPACE  FOR  TEMPORARY  FILES  (APPROXIMATELY 
C— 160K  BYTES/100  POINTS,  4MEG  BYTES  MAX  FOR  2500  POINTS). 

C 

C***************************  SECTION  1  ********************* 
***************  DECLARATION  OF  PARAMETERS  *********** 
INTEGER  ONE 

PARAMETER  (  ONE=l,  MAXOBS=250  ,  NDIMEN=3  ) 

PARAMETER  (  NUMSTA=3*NDIMEN,  NSTMAT=NUMSTA*NUMSTA , 

*  NOBVEC=NUMSTA*MAXOBS  ) 

***************  DECLARATION  OF  ARRAYS  ************ 

DOUBLE  PRECISION  XKK(NUMSTA) ,XKKM1 (NUMSTA) , 

*  ZZ (NUMSTA) ZKKMl (NUMSTA) , XNNMl (NUMSTA) , XPl (NUMSTA) 

DOUBLE  PRECISION  TMPVl (NUMSTA) ,TMPV2 (NUMSTA) 

DOUBLE  PRECISION  VTIME (MAXOBS ) , SOURCE (MAXOBS ) , 

*  ROLL (MAXOBS) 

DOUBLE  PRECISION  XKKS (NUMSTA, MAXOBS) 

C 


61 


o o o o o o o o o o n o o o o o o o  ooooooo  o  o  ooo 


DOUBLE  PRECISION  PHI (NUMSTA,NUMSTA) , 

*  Q(NUMSTA,NUMSTA) ,HI(NUMSTA,NUMSTA) , 

*  PKKMl (NUMSTA , NUMSTA) , PKK ( NUMSTA , NUMSTA) , 

*  H (NUMSTA, NUMSTA) ,HTRANS (NUMSTA, NUMSTA) , 

*  R (NUMSTA, NUMSTA) ,G (NUMSTA, NUMSTA) , 

*  PHIT (NUMSTA, NUMSTA) , PKKS (NUMSTA, NUMSTA) , 

*  ''PKKPl  (NUMSTA, NUMSTA)  ,AK (NUMSTA, NUMSTA)  , 

*  RPROJ (NUMSTA , NUMSTA) 

DOUBLE  PRECISION  TEMPI (NUMSTA, NUMSTA) , 

*  TEMP2 (NUMSTA, NUMSTA) , TEMP3 (NUMSTA, NUMSTA) 

***********  DECLARATION  OF  SCALARS  *************** 

DOUBLE  PRECISION  ACCTHRSH,ROLTHRSH,WTMIN, WTMAX 
COMMON  /CBLK/  PTC (MAXOBS ) , XYRANGE (3,2), 

*  PKKONEONE (MAXOBS ) , IRl 

INTEGER  KF,SRC 

CHARACTER  PKKS3D*13,  PKKM1S*13 

CHARACTER  ASK*1,  INFILC*13,  INFILR*13,  INFILP*13, 

*  INFILA*13,  OUTFIL*13 


*******  INITIALIZATION  OF  SELECTED  ARRAYS  ************ 
****.**  (THOSE  NOT  OTHERWISE  INITIALIZED)  ************* 

DATA  HI  /NSTMAT*0.D0/,  VTIME  /MAXOBS *0. DO/, 

*  XKKS  /NOBVEC*O.DO/,  SOURCE  /MAXOBS *0 . DO/ , 

*  PKKMl  /NSTMAT*0.D0/,  ROLL  /MAXOBS * 0 . DO/ 

**********************  SECTION  2  ************************* 


*****  designate  and  OPEN  INPUT  AND  OUTPUT  FILES  ******** 


WRITE (*,*)  'ENTER  NAME  OF  INPUT  RANGE  POSITION  DATA 
♦FILE' 

READ(50, ' (A) ')  INFILP 

WRITE (*,*)  'ENTER  NAME  OF  INPUT  INTERNAL  VELOCITY  DATA 
♦FILE' 


READ(51, ' (A) ')  INFILA 

WRITE (*,*)  'ENTER  NAME  OF  INPUT  INTERNAL  ACCELERATION 
♦DATA  FILE' 

READ(52, ' (A) ')  INFILC 

WRITE (♦,♦)  'ENTER  NAME  OF  INPUT  ROLL  ANGLE  DATA  FILE' 
READ(53, ' (A) ')  INFILR 

WRITE (♦,♦)  'ENTER  NAME  OF  SMOOTHED  DATA  FILE' 

READ ( 54 , ' ( A) ' )  OUTFIL 

OPEN ( 2 , FILE=  ' DATA\XKKFWD . DAT ' ,  STATUS= ' NEW ' ) 

OPEN ( 3 , FI LE=  ' DATA\XKSMOOTH . DAT ' , STATUS*  *  NEW ' ) 

OPEN ( 4 , FILE*  ' DATA\PKKOUT . DAT ' ,  STATUS* ' NEW ' ) 
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OPEN ( 11 , FILE* ' DATA\OBSERVAT.DAT ' , STATUS* • OLD ' ) 

PKKS3D  =  'DATA\$3DTEMP* 

PKKMIS  *  •DATA\$M1TEMP' 

OPEN ( 63 , FILE*PKKS3D,  ACCESS* • DIRECT ' , 

*  STATUS*  • NEW ' , FORM* ' FORMATTED • , RECL*1 3  5 ) 

OPEN ( 61 , FILE*PKKM1S ,  ACCESS* • DIRECT ' , 

*  STATUS*  ' NEW ' , FORM*  *  FORMATTED • , RECL*13  5 ) 

*********  INITIALIZE  SELECTED  COUNTERS  ************* 

IRl  *  1 
IR  *  1 
K  *  0 

****  INITIALIZE  ARRAY  DIMENSIONS  AND  VARIABLES  ****** 

L  =  1 
N  =  9 
NW  =  3 
ND  =  NUMSTA 
LD  =  ONE 

PKKM1(1,1)  =  1000000. ODO 
ROLTHRSH=5 . 0 
ACCTHRSH=5 . 0 
WTMIN=1800. 

WTMAX  =  20000.0 

*****  INITIALIZE  THE  IDENTITY  MATRIX  AND  X(l|0)  ****** 

DO  190  I  =  1,  9 
XKKMl(I)  =  0.0 
HI(I,I)  =  1. 

190  CONTINUE 

*****  echo  initial  FILTER  PARAMETERS  AND  ALLOW  ***** 
*****  USER  TO  CHANGE  THEM  IF  DESIRED.  ***** 

CALL  CHANGE (PKKM1,WTMIN, WTMAX, N) 

CALL  SETTHRSH ( ROLTHRSH , ACCTHRSH ) 

C 

C 

C  **********************  SECTION  3  ************************* 
C 

C  ****  READ  IN  THE  "RAW”  OBSERVATION  DATA  FILE,  WHICH  *** 

C  ****  HAS  BEEN  PROPERLY  FORMATTED  IN  AN  EARLIER  *** 

C  ****  E.G.  OBSDATA.FOR.  WHEN  THIS  DATA  IS  READ  IN  *** 

C  ****  THE  Kth  OBSERVATION  IS  ORIGINALLY  LOADED  INTO  *** 

C  ****  XKKS(*,K).  THEN,  WHEN  THE  FILTER  IS  RUN,  THIS  *** 

C  ****  VALUE  WILL  BE  REPLACED  WITH  THE  ESTIMATED  STATE  *** 

C  ****  X(KjK).  FINALLY,  WHEN  THE  BACKWARD  SMOOTHING  IS  *** 
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C  ****  RUN,  THIS  VALUE  WILL  BE  REPLACED  WITH  THE  FINAL  *** 
C  *****  SMOOTHED  VALUE.  *** 

C 

READ(11,210)  IRl 
IF  (  IRl  .GT.  MAXOBS  )  THEN 
WRITE (*,209)  IRl,  MAXOBS 
STOP 
ENDIF 
C 

DO  201  IPRT  =  1  IRl 

READ(11,211,END*300)  PTC(IPRT) ,VTIME(IPRT) , 

*  SOURCE ( IPRT) , (  XKKS ( JPRT , IPRT) , JPRT=1 , 9 ) , 

*  ROLL (IPRT) 

201  CONTINUE 

CLOSE (11) 

C 

209  FORMAT ('  The  Number  of  Observations  ',15,',  exceeds 
*the','  maximvun  storage  available  (',15,')'  ) 

210  F0RMAT(/,1X,I4,/) 

211  F0RMAT(13(1X,E14.8) ) 

300  CONTINUE 


********************  SECTION  4  ************************** 

********************************************************* 

****  IMPLEMENT  THE  (FORWARD)  KAUIAN  FILTER  ****** 

********************************************************* 


100  K  =  K  +  1 

WRITE (*,*)  'Forward  Filter  -  Step',K 

***  BYPASS  COMPUTATION  OF  P(l|0)  AND  X(ljO)  *** 

***  DURING  FIRST  ITERATION  *** 

IF  (  K  .EQ.  1  )  GO  TO  101 

***  GENERATE  THE  APPROPRIATE  STATE  TRANSITION  *** 
***  MATRIX  (  PHI  )  AND  ITS  TRANSPOSE,  BASED  ON  *** 
***  the  elapsed  time  since  the  last  OBSERVATION.*** 

DT  =  VTIME(K)-VTIME(K-1) 

CALL  PHIDEL(DT,NW,PHI,ND) 

CALL  TRANS ( PHI, N,N,PHIT,ND,ND) 

****  DETERMINE  WHETHER  THE  FILTER  BELIEVES  THE  **** 
****  TORPEDO  IS  MANEUVERING,  AND  GENERATE  THE  **** 
****  APPROPRIATE  COVARIANCE  OF  EXCITATION  MATRIX  **** 

CALL  MANDET (ROLL, XKKS , K, ROLTHRSH , ACCTHRSH , WTMIN , 

*  WTMAX,DT,Q,ND,NW) 
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*** 


***  FORM  P(KjK-l),  BASED  ON  THE  EQUATION 

P(K|K-1)  =  PHI*P(K-1|K-1)*TRANS{PHI}  +  Q(K) 

***  WHERE  P(K-1|K-1)  IS  P(K|K)  FROM  THE  *** 

***  PREVIOUS  STEP.  *** 

CALL  PROD ( PKK , PHIT , N , N , N , TEMPI , ND , ND , ND) 

CALL  PROD(PHI, TEMPI, N,N,N,TEMP2,ND,ND,ND) 

CALL  ADD(TEMP2,Q,N,N,PKKM1,ND,ND) 

***  AND  ALSO  X(K|K-1)  =  PHI*X(K-1 | K-1)  *** 

***  (  WHERE  X(K-ljK-l)  IS  X(K[K)  FROM  THE  *** 

***  PREVIOUS  STEP.)  *** 

CALL  PROD ( PHI , XKK , N , N , ONE , XKKMl , ND , ND , ONE ) 

101  CONTINUE 

****  determine  THE  SOURCE  OF  THIS  OBSERVATION  **** 

****  (E.G.  POSITION  ONLY,  VELOCITY  ONLY,  **** 

****  POSITION  AND  VELOCITY,  ETC.)  AND  FORM  THE  **** 

****  CORRESPONDING  MEASUREMENT  MATRIX  (  H(K)  )  **** 

****  AND  ITS  TRANSPOSE  **** 

SRC  =  SOURCE (K) 

CALL  BUILDH(SRC,M,H,NW,ND) 

CALL  TRANS (H,M,N,HTRANS,ND,ND) 

***  DETERMINE  THE  FULL  (RANGE-DEPENDENT)  *** 

***  COVARIANCE  OF  MEASUREMENT  NOISE  MATRIX  (  R  )  *** 

***  THEN  PROJECT  IT  ONTO  THE  CURRENT  OBSERVATION  *** 

***  VECTOR  COMPONENTS  *** 

CALL  BUILDR(R,NW,ND) 

CALL  PROD(H,R,M,N,N, TEMPI, ND,ND,ND) 

CALL  PROD ( TEMPI , HTRANS , M , N , M , RPROJ , ND , ND , ND ) 

****  USE  THE  MEASUREMENT  MATRIX  (  H  )  TO  SELECT  **** 

****  THE  CORRECT  COMPONENTS  (  Z(K)  )  FROM  THE  **** 

****  "RAW"  OBSERVATION  ARRAY  (  XKKS  ) .  **** 

CALL  PROD (H, XKKS (1,K) ,M,ND,ONE, ZZ,ND,ND,ONE) 

****  COMPUTE  THE  OPTIMUM  GAIN  MATRIX  G(K)  **** 

****  BASED  ON  THE  FORMULA  **** 

G(K)  = 
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P(K|K-1)*TRANS{H(K) )*INV{ [H(K)*P(K|K-1)*TRANS{H(K) }  +  R] } 

CALL  PROD (PKKMl , HTRANS , N, N, M, TEMPI , ND, ND, ND) 

CALL  PROD ( H , TEMPI , M , N , M , TEMP2 , ND , ND , ND) 

CALL  ADD (TEMP2 , RPROJ , M , M , TEMP3 , ND , ND) 

CALL  RECIP(TEMP3,TEMP2,M,ND) 

CALL  PROD (TEMPI, TEMP2,N,M,M,G,ND,ND,ND) 


****  CALCULATE  Z(K|K-1)  BASED  ON  THE  FORMULA  **** 
****  Z(KjK-l)  *  H(K)*X(K|K-1)  **** 

CALL  PROD(H,XKKMl,N,N,ONE,ZKKMl,ND,ND,ONE) 

****  SOLVE  FOR  THE  UPDATED  STATE  ESTIMATE  VECTOR  **** 
****  (  X(K|K)  ),  BASED  ON  THE  EQUATION  **** 

****  X(K|K)  =  X(KjK--l)  +  G(K)*[Z(K)  -  Z(KjK-l)]  **** 
****  WHERE  Z(K)  IS  THE  CURRENT  OBSERVATION  **** 


CALL  SUB ( Z  Z , Z KKMl , M , ONE , TMPVl , ND , ONE ) 

CALL  PROD ( G , TMPVl , N , M , ONE , TMPV2 , ND , ND , ONE ) 

CALL  ADD ( XKKMl , TMPV2 , N , ONE , XKK , ND , ONE ) 

***  NOW  COMPUTE  THE  COVARIANCE  OR  ERROR  ESTIMATE  *** 
***  MATRIX  P(KjK)  ,  BASED  ON  THE  EQUATION;  *** 

P(KjK)  =  {  I  -  G(K)*H(K)  )*P(K|K-1) 

CALL  PROD(G,H,N,M,N, TEMPI, ND,ND,ND) 

CALL  SUB ( HI , TEMPI , N , N , TEMP2 , ND , ND ) 

CALL  PROD (TEMP2 , PKKMl , N , N , N , PKK , ND , ND , ND) 

***  STORE  THE  FILTERED  DATA  IN  XKKS(*,K)  (FOR  *** 
***  SUBSEQUENT  SMOOTHING  (BACKWARDS  FILTERING)  *** 
***  AND  ALSO  SAVE  X(K|K),  P(K!K),  AND  P(K|K-1)  *** 

DO  50  1=1,9 

50  XKKS(I,K)  =  XKK(I) 

WRITE(2,602)  VTIME(K) ,XKKS(1,K) ,  XKKS(4,K),  XKKS(7,K) 
WRITE(4,900)  PKK(1,1) ,PKK(1,4) ,PKK(4,1) ,PKK(4,4) 

KREC  =  9*(K-1)+1 
WRITE(61,800,REC=KREC)  PKKMl 
WRITE(63,800,REC=KREC)  PKK 

602  F0RMAT(4(1X,E14.8) ) 

800  F0RMAT(9(1X,D14.8) ) 

900  F0RMAT(9(1X,E14.8)  ) 

****  AND  CONTINUE  UNTIL  ALL  OBSERVATIONS  ARE  FILTERED  **** 


IF  (K.LT.IRl)  GOTO  100 


ooooo  DO  anno  nan  n n n n n n n n n n n n n 


c 


CLOSE (2) 
CLOSE (4) 
CLOSE (61) 
CLOSE (63) 


***********************  SECTION  5  ************************ 

********************************************************** 

*  IMPLEMENT  THE  SMOOTHING  (BACKWARD  FILTERING)  ROUTINE  * 

********************************************************** 


*****  OPEN  THE  REQUIRED  DATA  FILES,  AND  INITIALIZE  **** 

*****  the  smoothed  covariance  of  error  estimate  **** 

*****  MATRIX  TO  P(IR1}IR1)  **** 

OPEN(63,FILE=PKKS3D,ACCESS=*DIRECT' ,STATUS='OLD' , 

*  FORM= ’ FORMATTED ' , RECL=1 3  5 ) 

OPEN ( 6 1,FILE=PKKM1S,ACCESS=' DIRECT ' ,STATUS=' OLD* , 

*  FORM* ' FORMATTED ' , RECL=13  5 ) 

KF=9*(IR1-1)+1 
READ(63,800,REC=KF)  PKKS 

*******  then  begin  the  backward  smoothing  ******** 

DO  600  K=1,IR1  -  1 

KI=  IRl  -  K  '  . 

WRITE (*,*)  'Backward  Smoothing  ~  step  ',KI 

*****  generate  phi  matrix  and  RETRIEVE  THE  STORED  *** 
*****  VALUES  OF  P(K,K),  AND  P(K+1!K)  *** 

DT  =  VTIME(KI+1)  -  VTIME(KI) 

CALL  PHIDEL(DT,NW,PHI,ND) 


KF=  1+9*(KI-1) 
READ(63,800,REC=KF)  PKKS 
READ(61,800,REC=KF+9)  PKKPl 


****  NOW  COMPUTE  THE  SMOOTHING  MATRIX,  BASED  ON  THE  **** 
****  EQUATION:  -1  **** 
****  A(K)  *  P(K|K)*TRANS{PHI)*P(K+1|K)  **** 


CALL  TRANS ( PHI, N,N,PHIT,ND,ND) 

CALL  RECIP( PKKPl, TEMPI, N,ND) 

CALL  PROD (PHIT, TEMPI, N,N,N,TEMP2,ND,ND,ND)  ^ 
CALL  PROD(PKKS,TEMP2,N,N,N,AK,ND,ND,ND) 
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hhltlt 

**** 


C  ******  SOLVE  FOR  SMOOTHED  ESTIMATE 

C  ******  XS(K)  =  X(KjK)  +  A(K)*[XS(K+1)  -  X(K+ljK)] 

C 

DO  504  I  =  1,9 

XPl(I)  =  XKKS(I,KI) 

XNNMl(I)  *  XKKS(I,KI+1) 

504  CONTINUE 

CALL  PROD ( PHI , XPl , N , N , ONE , TMPVl , ND , ND , ONE ) 

CALL  SUB ( XNNMl , TMPVl , N , ONE , TMPV2 , ND , ONE ) 

CALL  PROD ( AK , TMPV2 , N , N , ONE , TMPVl , ND , ND , ONE) 

CALL  ADD ( XPl , TMPVl , N , ONE , TMPV2 , ND , ONE ) 

DO  505  I  =  1,9 

XKKS(I,KI)  =  TMPV2(I) 

505  CONTINUE 

*****  and  update  the  smoothed  covariance  of  error  **** 

*****  ESTIMATE  MATRIX  BASED  ON  THE  EQUATION  **** 

*****  PS(KjK)  =  P(K|K)  +  **** 

*****  A(K) *[PS(K+1 jK+1)  -  P(K+ljK) ]*TRANS{A(K) )  **** 


CALL  SUB ( PKKS , PKKPl , N , N , TEMPI , ND , ND) 

CALL  TRANS (AK,N,N,TEMP2,ND,ND) 

CALL  PROD ( TEMPI , TEMP2 , N , N , N , TEMP3 , ND , ND , ND ) 
CALL  PROD ( AK , TEMP3 , N , N , N , TEMPI , ND , ND , ND) 
CALL  ADD ( PKKS , TEMPI , N , N , PKKS , ND , ND ) 


*****  and  continue  until  all  records  ARE  SMOOTHED 
600  CONTINUE 


**** 


*****  SAVE  THE  SMOOTHED  DATA 


**** 


DO  601  KI  =  1,IR1 

WRITE (3, 602)  VTIME(KI) ,XKKS(1,KI) ,XKKS(4,KI) , 
*  XKKS(7,KI) 

601  CONTINUE 


*****  CLOSE  ALL  FILES  AND  EXIT 


**** 


CLOSE (3) 

CLOSE ( 63 , STATUS= ' DELETE ’ ) 
CLOSE ( 61 , STATUS= ‘ DELETE ' ) 

STOP 

END 


C***********************  SECTION  6  ************************* 

Q  It************************************************ ********* 

C  **  REQUIRED  SUBROUTINES  ** 

Q  ********************************************************** 
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********************************************************** 
SUBROUTINE  WHICH  COMPUTES  THE  PHI  MATRIX 
THE  MATRIX  IS  BUILD  OF  SUCCESSIVE  BLOCKS,  ONE 
PER  COMPONENT  DIRECTION  OBSERVED,  WHERE  EACH 
BLOCK  HAS  THE  FORM: 

{  1  T  T^2/2  I 

0  1  T 

I  0  0  1  ! 

THIS  PROCEDURE  ASSUMES  THE  COMPONENTS  OF  THE  "RAW" 

STATE  VECTOR  ARE  T 

[  X  X'  X"  Y  Y'  Y"  Z  Z*  Z"  _  ] 

******************************************** 


SUBROUTINE  PHIDEL(T,NW,PHI,ND) 
DOUBLE  PRECISION  PHI(ND,ND) 


IF  (  3*NW  .GT.  ND  )  THEN 
WRITE(*,201)  NW,  ND 
STOP 
ENDIF 


DO  100  IR  =  1,NW 

IBLK  =  3*(IR-1) 

PHI ( I BLK+ 1 , I BLK+ 1 ) 
PHI(IBLK+l,IBLK+2) 
PHI ( IBLK+1 , IBLK+3 ) 
PHI(IBLK+2,IBLK+2) 
PHI (IBLK+2, IBLK+3) 
100  PHI (IBLK+3, IBLK+3) 

RETURN 


1. 

T 

T*T/2.D0 

1. 

T 

1. 


201  FORMAT ('  ****  ERROR  IN  SUBROUTINE  PHIDEL  -  3*NW 

*  (=',13,')','  .GT.  ND  (=M3,  •)  •) 

END 


********************************************************** 

SUBROUTINE  WHICH  ADDS  TWO  INPUT  MATRICES 

********************************************************** 


SUBROUTINE  ADD(A, B,N,M,C,ND,MD) 

DOUBLE  PRECISION  A(ND,MD) , B(ND,MD) , C(ND,MD) 

IF  (  (  N  .GT.  ND  )  .OR.  (  M  .GT.  MD  )  )  THEN 
WRITE(*,201)  N,  ND,  M,  MD 
STOP 
ENDIF 

DO  100  I  *  1,N 
DO  100  J  =  1,M 
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100 


C(I,J)  =  A(I,J)  +  B(I,J) 

RETURN 
C 

201  FORMATC  ****  ERROR  IN  SUBROUTINE  ADD  -  •,/,10X, 

*  'Either  N  (=',I3,')','  .GT.  ND  (=' ,13, ') • ,/,10X, 

*  'or  M  (=',I3,')','  .GT.  MD  (=',13,')') 

END 

A********************************************************* 

SUBROUTINE  WHICH  SUBTRACTS  THE  SECOND  INPUT  MATRIX  FROM 

THE  FIRST 

********************************************************** 


SUBROUTINE  SUB(A,B,N,M,C,ND,MD) 

DOUBLE  PRECISION  A(ND,MD) ,B(ND,MD) ,C(ND,MD) 

IF  (  (  N  .GT.  ND  )  .OR.  (  M  .GT.  MD  )  )  THEN 
WRITE(*,201)  N,  ND,  M,  MD 
STOP 
ENDIF 

DO  100  I  =  1,N 
DO  100  J  =  1,M 

100  C(I,J)  =  A(I,J)  -  B(I,J) 

RETURN 

201  FORMATC  ****  ERROR  IN  SUBROUTINE  SUB  -  ',/,10X, 

*  'Either  N  (=',13,')','  .GT.  ND  (=' ,13, ')',/, lOX, 

*  'or  M  (=',13,')','  .GT.  MD  (=',13,')') 


END 

************************************************* 

SUBROUTINE  WHICH  MULTIPLIES  TWO  INPUT  MATRICES 

************************************************* 

SUBROUTINE  PROD ( A , B , N , M , L , C , ND , MD , LD) 

DOUBLE  PRECISION  A(ND,MD) , B(MD, LD) , C(ND, LD) 

IF  (  (  N  .GT.  ND  )  .OR.  (  M  .GT.  MD  )  .OR. 

*  (  L  .GT.  LD  )  )  THEN 

WRITE(*,201)  N,  ND,  M,  MD,  L,  LD 
STOP 
ENDIF 

DO  100  I  =  1,N 
DO  100  J  =  1,L 
C(I,J)  =  O.DO 
DO  100  K  =  1,M 

C(I,J)  =C(I,J)  +  A(I,K)*B(K,J) 
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100  CONTINUE 
RETURN 

201  FORMATC  ****  ERROR  IN  SUBROUTINE  PROD  -  ',/,10X, 

*  'Either  N  (=',13,')','  .GT.  ND  (=', 13 lOX, 

*  'or  M  (=',13,')','  .GT.  MD  (=',I3, ') ',/,10X, 

*  'or  L  (=',13,')','  .GT.  LD  (=',13,')') 


END 

SUBROUTINE  WHICH  COMPUTES  THE  TRANSPOSE  OF  THE  INPUT 

MATRIX 

***********  *********************************************** 

SUBROUTINE  TRANS (A,N,M,C,ND,MD) 

DOUBLE  PRECISION  A(ND,MD) ,C(MD,ND) 

IF  (  (  N  .GT.  ND)  .OR.  (  M  .GT.  MD)  )  THEN 
WRITE(*,201)  N,  ND,  M,  MD 
STOP 
ENDIF 

DO  100  I  =  1,N 
DO  100  J  =  1,M 

C(J,I)  =  A(I,J) 

100  CONTINUE 
RETURN 

201  FORMATC  ****  ERROR  IN  SUBROUTINE  TRANS  -  \/,10X, 

*  'Either  N  (=',13,')','  .GT.  ND  (=', 13 ,')',/, lOX, 

*  'or  M  (=',13,')','  .GT.  MD  (=',13,')') 

END 

********************************************************* 
SUBROUTINE  WHICH  CALCULATES  THE  INVERSE  OF  THE  INPUT 

MATRIX 

********************************************************* 
SUBROUTINE  RECIP(A,C,N,ND) 

DOUBLE  PRECISION  A(ND,ND) ,C(ND,ND) ,D(20, 20) ,TEMP 

IF  (  N  .GT.  ND  )  THEN 
WRITE(*,801)  N,  ND 
STOP 
ENDIF 
C 

DO  100  I  =  1,N 
DO  100  J  =  1,N 
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D(I,J)  =  A(I,J) 


DO  115  I  =  1,N 
DO  115  J  =  N+1,2*N 
115  D(I,J)  =  0.0 
C 

DO  120  I  =  1,N 
J  =  I  +  N 
120  D(I,J)  =  1.0 
C 

DO  240  K  =  1,N 
M  =  K  +  1 

IF  (K  .EQ.  N)  GOTO  180 
L  =  K 

DO  140  I  =  M,N 

140  IF  (ABS(D(I,K))  .GT.  ABS(D(L,K)))  L  =  I 
IF  (L  .EQ.  K)  GOTO  180 
DO  160  J  =  K,2*N 
TEMP  =  D(K,J) 

D(K,J)  =  D(L,J) 

160  D(L,J)  =  TEMP 

180  DO  185  J  =  M,2*N 

185  D(K,J)  =  D(K, J)/D(K,K) 

IF  (K  .EQ.  1)  GOTO  220 
Ml  =  K  -  1 
DO  200  I  =  1,M1 
DO  200  J  =  M,2*N 

200  D(I,J)  =  D(I,J)  -  D(I,K)*D(K,J) 

C 

IF  (K  .EQ.  N)  GOTO  260 
220  DO  240  I  =M,N 

DO  240  J  =  M,2*N 

240  D(I,J)  =  D(I,J)  -  D(I,K)*D(K,J) 

260  DO  265  I  =  1,N 
DO  265  J  =  1,N 
K  =  J  +  N 

265  C(I,J)  =  D(I,K) 

RETURN 

C 

801  FORMAT ('  ****  ERROR  IN  SUBROUTINE  RECIP  -  N 

*(=M3,  '  .GT.  ND  (=M3,  •)  ') 

END 


********************************************************** 
SUBROUTINE  WHICH  ALLOWS  THE  USER  TO  CHANGE  W  AND  PKKMl 
TO  ALTER  THE  FILTER  BEHAVIOR  WITHOUT  HAVING  TO  RECOMPILE 
THE  PROGRAM 

********************************************************** 
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c 


c 


c 


c 


c 


SUBROUTINE  CHANGE (PKKM1,WTMIN,WTMAX,ND) 

DOUBLE  PRECISION  PKKM1(ND,ND) 

REAL*8  X,WTMIN,WTMAX 

WRITE(*,21)  PKKM1(1,1) 

WRITE (*,22) 

READ(*,23,END=24)  X 
IF  (  X  .LE.  0.  )  GOTO  24 
PKKM1(1,1)  =  X 
WRITE(*,25)  PKKM1(1,1) 

24  CONTINUE 

DO  26  1=1,9 

PKKMl ( I , I ) =PKKM1 ( 1 , 1) 

26  CONTINUE 

21  FORMAT (/,'  The  Covariance  of  Error  Estimate  Matrix  - 

*  ', 'currently  has  the  value: • ,//,10x, 'P(I,I)  = 

*  ',F14.4,//,'  A  large  value  (approx.  10**6)  will 

*  produce  ','a  fast  initial  response.',/, 

*  'Decreasing  one  or  more  decades  from  this  value 

*  will  ','slow  the  initial  response') 

22  FORMAT ('  You  may  simply  hit  "Enter"  to  accept  this 

*  value,',',  or',/,'  *>’"*er  a  new  value  now  (being 

*  sure  to  include  tuv  aecimal  point) ' ) 

23  FORMAT (D14. 5) 

25  format!'  Covariance  of  Error  Estimate  Matrix  reset 

*  to','  -  ',D14.5) 

WRITE (*,900)  WTMIN 
WRITE (*,22) 

READ(*,23,END=910)  X 
IF  (  X  .LE.  0.  )  GOTO  910 
WTMIN  =  X 
WRITE(*,901)  WTMIN 

900  FORMAT (/, '  The  current  MINIMUM  weight  on  the  Q  matrix 

*  for  a  ', 'maneuver  is:  ',F9.3) 

901  FORMAT ('  MINIMUM  weight  on  the  Q  matrix  for  a  ', 

*  'maneuver  reset  to:  ',F9.3) 

910  WRITE (*,905)  WTMAX 
WRITE(*,22) 

READ(*,23,END=920)  X 
IF  (  X  .LE.  0  )  GOTO  920 
WTMAX  =  X 

WRITE(*,906)  WTMAX 
920  CONTINUE 

905  FORMAT (/,'  The  current  MAXIMUM  weight  on  the  Q  matrix 

*  for  a  ', 'maneuver  is:  ',F9.3) 

906  FORMAT ('  MAXIMUM  weight  on  the  Q  matrix  for  a  ', 

*  'maneuver  reset  to:  ',F9.3) 
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C 


RETURN 

END 


********************************************************** 

SUBROUTINE  WHICH  ALLOWS  USER  TO  CHANGE  THRESHOLDS 

********************************************************** 

SUBROUTINE  SETTHRSH(ATHRSH,RTHRSH) 

REAL* 8  ATHRSH,RTHRSH,TMP 

WRITE (*,10)  ATHRSH 
WRITE(*,22) 

READ(*,50,END=25)  TMP 
IF  (  TMP  .LE.  0.  )  GOTO  25 
ATHRSH  =  TMP 
WRITE (*,11)  ATHRSH 

10  FORMAT (/, '  The  current  Acceleration  Threshold 

*  is: ' , F8 . 3) 

11  FORMAT ('  Acceleration  Threshold  reset  to:',F8.3) 

22  FORMAT  ('  You  may  simply  hit  "Enter''  to  accept  this 

*  value, ' , 

*  ',  or',/,'  enter  a  new  value  now  (being  sure  to 

*  include  the  decimal  point) ' ) 

50  FORMAT (D12. 3) 

25  WRITE (*,30)  RTHRSH 
WRITE(*,22) 

READ(*,50,END=40)  TMP 
IF  (  TMP  .LE.  0.0  )  GOTO  40 
RTHRSH  =  TMP 
WRITE (*,31)  RTHRSH 

30  FORMAT(/,'  The  current  Roll  Threshold  is:',F8.3) 

31  FORMATC  Roll  Threshold  reset  to:',F8.3) 

40  CONTINUE 
RETURN 
END 


********************************************************** 

SUBROUTINE  TO  DETECT  IF  A  MANEUVER  IS  OCCURRING  AND  SET  Q 
APPROPRIATELY.  THIS  SUBROUTINE  ASSUMES  THE  COMPONENTS  OF 
THE  "RAW"  STATE  VECTOR  ARE 


T 

[  X  X'  X"  Y  Y'  Y"  Z  Z'  Z"  . . . .  ] 

AND  BUILDS  THE  Q  MATRIX  BLOCK  BY  BLOCK,  ACCORDING  TO 
THE  PATTERN: 
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Q(l,l)  *  (DT**6/36)  *  WT 
Q(2,2)  =  (DT**4/4)  *  WT 
Q(3,3)  =  DT**2  *  WT 

BUT  CHANGING  THE  WEIGHTS  IN  EACH  BLOCK  ACCORDING  TO 
WHETHER  THE  TORPEDO  APPEARS  TO  BE  MANEUVERING  IN  THAT 
DIRECTION. 

********************************************************** 

SUBROUTINE  MANDET (RL, ZK, K, RTH, ATH , WTMIN , WTMAX , 

*  DT,Q,ND,NW) 


DOUBLE  PRECISION  RL(1) ,ZK(ND, 1) ,Q(ND,ND) 

DOUBLE  PRECISION  RTH,ATH, WTMIN, WTMAX, WT 
INTEGER  K,ND,NW 

IF  (  3*NW  .GT.  ND  )  THEN 
WRITE(*,201)  3*NW,  ND 

STOP 
ENDIF 

DO  100  ICOM=l,NW 
WT=WTMIN 

IF(  (  ABS(RL(K))  .GT.RTH  )  .OR. 

*  (  ABS(ZK(3*IC0M,K) )  .GT.  ATH  )  )  WT=WTMAX 
IBLK  =  3*ICOM 

Q(IBLK-2,IBLK-2)  =  (DT**6) *WT/36. DO 
Q(IBLK-1,IBLK-1)  =  (DT**4) *WT/4 . DO 
Q(IBLK,IBLK)  =  (DT**2)*WT 

100  CONTINUE 
RETURN 

201  FORMAT ('  ****  ERROR  IN  SUBROUTINE  MANDET  -  3*NW 

*  (=',I3,')','  .GT.  ND(=M3,')') 

END 


**************************************************«*««A«^A 

SUBROUTINE  TO  BUILD  THE  RANGE  DEPENDENT  COVARIANCE  OF 
MEASUREMENT  MATRIX  -  R  (TBD) 

THIS  SUBROUTINE  ASSUMES  THE  COMPONENTS  OF  THE  ''RAW” 

STATE  VECTOR  ARE  T 

[  X  X'  X”  Y  Y'  Y”  Z  Z'  Z”  -  ] 

*************************************************** 

SUBROUTINE  BUILDR(R,NW,ND) 

C 

DOUBLE  PRECISION  R(ND,ND) 

INTEGER  NW 
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IF  (  3*NW  .GT.  ND  )  THEM 
WRITE(*,201)  3*NW,  ND 

STOP 
ENDIF 

*****  ZERO  OUT  ANY  PREVIOUS  VALUES  ******* 

DO  90  IVAR  =  1,ND 
DO  90  JVAR  =  1,ND 

R(IVAR,JVAR)  =  O.DO 
90  CONTINUE 

DO  100  IVAR  =  1,  NW 

R(3*IVAR-2,3*IVAR-2)  =  15.0D0 
R(3*IVAR-1,3*IVAR-1)  *  .03D0 
R(3*IVAR,3*IVAR)  =  .09D0 

100  CONTINUE 
RETURN 

201  FORMAT ('  ****  ERROR  IN  SUBROUTINE  BUILDR  -  3*NW 

*  (=M3,')','  .GT.  ND  (=M3,')') 

END 

********************************************************** 

SUBROUTINE  TO  BUILD  THE  MEASUREMENT  MATRIX  ~  H 

THIS  SUBROUTINE  ASSUMES  THE  COMPONENTS  OF  THE  ''RAW 
STATE  VECTOR  ARE  T 

[  X  X'  X”  Y  Y'  Y"  Z  Z'  Z”  . . . .  ] 

SRC  IS  INTERPRETED  AS  A  BINARY  NUMBER,  WHERE  A  ''I"  IN 
A  GIVEN  POSITION  MEANS  THAT  COMPONENT  IS  ALSO  PRESENT  IN 
THE  CORRESPONDING  OBSERVATION,  E.G. 

SRC  =  23  =  (000010111)  (binary) 

IMPLIES  X(l) ,X(2) ,X(3)  AND  X(5)  MAKE  UP  THE  OBSERVATION. 
********************************************************** 


SUBROUTINE  BUILDH(SRC,M,H,NW,ND) 

DOUBLE  PRECISION  H(ND,ND) 

INTEGER  NW , ND , SRC , M , IROW 

INTEGER  BINCODE(9)  /I, 2 , 4 , 8, 16, 32 , 64 , 128 , 256/ 

IF  (  (  3*NW  .GT.  ND  )  .OR.  (  ND  .GT.  9  )  )  THEN 
WRITE(*,201)  3*NW,  ND 

STOP 
ENDIF 

*****  ZERO  OUT  ANY  PREVIOUS  VALUES  ******* 


DO  90  IVAR=1,ND 
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DO  90  JVAR=1,ND 

H(IVAR,JVAR)  *  O.DO 
90  CONTINUE 
C 

IROW  =  0 

*******  INCLUDE  OBSERVATIONS  AS  APPROPRIATE  ***** 
*******  BASED  ON  WHETHER  THE  CORRESPONDING  ***** 
*******  binary  digit  is  present  in  SRC  ***** 

DO  100  IVAR=1,ND 

IF  (  MOD(  SRC/BINCODE(IVAR)  ,2)  .EQ.  1  )  THEN 
IROW  *  IROW  +  1 
H(  IROW,IVAR  )  »  l.DO 
ENDIF 

100  CONTINUE 


C 

M  =  IROW 
RETURN 
C 

201  FORMAT ('  ****  ERROR  IN  SUBROUTINE  BUILDH  -  3*NW 

*  (=M3,')'/'  -GT.  ND  (=',13,').  OR  ND  >  9') 

END 
AZ 
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